CN113984080A

CN113984080A - Hierarchical local path planning method suitable for large complex scene

Info

Publication number: CN113984080A
Application number: CN202111250885.7A
Authority: CN
Inventors: 郑南宁; 张崧翌; 简志强; 邓晓东; 陈仕韬
Original assignee: Xian Jiaotong University
Current assignee: Xian Jiaotong University
Priority date: 2021-10-26
Filing date: 2021-10-26
Publication date: 2022-01-28
Anticipated expiration: 2041-10-26
Also published as: CN113984080B

Abstract

The invention discloses a hierarchical local path planning method suitable for a large-scale complex scene, which comprises the steps of obtaining a local grid map with the current position of a vehicle as the center, and marking an obstacle in the local grid map; generating a global guide path and extracting key points; on the basis of a hybrid A method, a layered search process, a pre-guiding path and a local end point are introduced to generate and improve a heuristic function expansion strategy and a constraint strategy based on orientation consistency; by searching the guide path in the coarse scale, the accuracy of the heuristic function in the fine scale is effectively improved, so that the time consumption for searching is reduced; in a specific searching method, the problem of repeated expansion is avoided by introducing a double-priority queue; meanwhile, the generation of abnormal paths is avoided due to the orientation limitation; by introducing the expanded search space and the spiral search element, the planned path has the characteristic of smooth curvature, and the vehicle following is facilitated.

Description

Hierarchical local path planning method suitable for large complex scene

Technical Field

The invention belongs to the field of autonomous driving vehicles, and particularly relates to a hierarchical local path planning method suitable for a large-scale complex scene.

Background

Autonomous driving techniques have been applied in part in our lives, but within some large complex environments, it is still difficult for an autonomously driven vehicle to achieve a similar level of driving as a human driver. The quality of the automatic driving effect is greatly influenced by the path planning module. In the face of the motion constraints of the vehicle, the safety and comfort requirements of the passengers inside the vehicle, the planning process must have a high real-time, while the generated path must have a high smoothness.

Existing planning methods are divided into two categories, structured map dependent planning and planning in free space. The former can improve the quality of path planning by optimizing a map in advance, but can only be applied to scenes with definite structured information. The latter explores the environment by means of graph search, but the real-time performance of search and the smoothness of the path are often difficult to satisfy.

Current researchers have proposed a hybrid a-method based on a-path search and straight lines and circles as primitives that can generate continuous oriented paths from start to end poses for vehicle tracking. However, for large scenes, this method still faces the problem of too slow a search speed. Meanwhile, since the search result does not have curvature continuity, abrupt changes in curvature still affect the passenger experience during the vehicle execution process.

Disclosure of Invention

In order to solve the problems in the prior art, the invention provides a rapid local path generation method facing a large-scale scene, the invention applies a modified hybrid A method to complete rapid and safe local path planning in the large-scale complex scene, and the search speed of the hybrid A method in the complex scene is greatly improved by introducing guide points obtained by primary global path planning, improved heuristic functions and a limited curve expansion mode.

In order to achieve the purpose, the invention adopts the technical scheme that: a hierarchical local path planning method suitable for a large complex scene comprises the following steps:

s100, acquiring a local grid map with the current position of a vehicle as the center, and marking an obstacle in the local grid map;

s200, generating a global guide path and extracting key points;

s300, acquiring a local end point meeting the global end point requirement based on the modified mixed A-x algorithm:

s301, defining a structure of a search node, and obtaining a heuristic function h and an adjacent search density of the search node based on the structure;

s302, an OPEN table and a BUFF table are constructed, the OPEN table is used for storing nodes to be explored, and the BUFF table is used for temporarily storing points needing to adjust and expand the priority order according to the density; setting initial density discrimination threshold rho at the same time_thWhen the current position of the vehicle is equal to 0, only the node corresponding to the current position of the vehicle is loaded in the OPEN table, and the BUFF table is left empty;

s303, determining a current node according to the points in the OPEN table and the BUFF table;

s304, according to

Updating the density threshold ρ_th；

S305, if the current node is from the OPEN table, and ρ (S)_BUFF)＞ρ_thIf yes, the current node is loaded into the BUFF table, and then the step is skipped to S303;

s306, judging whether the current node meets the requirement of the global end point, and jumping to S400 if the current node meets the requirement of the global end point;

s307, if the current node does not meet the requirement of the global end point, judging whether the current node can be linked to the next local end point through an RS curve, if so, creating the RS curve and constructing a father node relation, adding the next local end point to an OPEN table, and jumping to S303;

s308, performing child node expansion on the current node to obtain an expansion curve; specifically, the motion primitive obtained and expanded by the vehicle kinematic equation integration is a straight line, a circle or a spiral and corresponds to a new vehicle position (x)_b,y_b) Direction of θ_bCurvature kappa_bAnd old node vehicle position (x)_a,y_a) Direction of θ_aCurvature kappa_aBased on the spiral primitive transition, the line connects the straight line and the circular arc;

s309, generating position and orientation information of the child nodes according to the type of the expansion curve;

s310, calculating a heuristic function value from the sub-node to the end point according to the position and orientation information, storing the heuristic function value as a node h value, updating an OPEN table by adopting a traditional A and mixed A method, and jumping to S303;

s400, constructing a complete path from the end point based on the local end point.

Acquiring a local grid map with the current position of the vehicle as the center: and obtaining a local grid map taking the current position of the vehicle as the center by rotating and cutting the global map based on the position and the orientation of the anchor point of the pre-stored global map in the world coordinate system and the position and the orientation of the current position of the vehicle output by the positioning module in the same world coordinate system.

Marking an obstacle in the local grid map: setting a height threshold value in a local grid map, and removing a ground point set in laser radar point cloud of the local grid map according to the height threshold value to obtain a non-ground point set in the laser radar point cloud; marking the corresponding position of the non-ground point set in the local grid map as an obstacle.

The step S200 of generating a global guidance path and extracting key points specifically includes the following steps:

s201 maps the end point of the vehicle local plan in the world coordinate system to a certain pixel position in the local grid map,

s202, applying A-algorithm to generate a complete guide path P from the current position of the vehicle to the terminal point_raw，

S203, analyzing the complete guide path P_rawConstructing rays in eight directions until the rays intersect with the barrier in the local grid map to obtain a set P of the midpoint of a narrow channel,

s204, refining the orientation and the channel width of each point in the set P to obtain the refined channel orientation and the half-boundary distance,

s205 moves the channel point to be at the center of the channel in the vertical direction according to the channel orientation and the boundary distance obtained by the refinement.

Application of A-algorithm to generate guidance path P from current position of vehicle to terminal_rawIn the process, after a complete guide path is obtained by adopting search expansion of 8 nearby grids in the peripheral direction based on an A-x algorithm, points which are far away from an obstacle and are smaller than the length of a vehicle are used as candidate points, and the guide path is marked as P_raw(ii) a When the orientation and the channel width of each point in the set P are refined, for the adjacent first orientation and second orientation, the first detection ray and the second detection ray are calculated through trigonometric relation to obtain the refined channel orientation and half-boundary distance.

The structure of the search node is specifically as follows: the position, the orientation and the track curvature of a vehicle reference point under a local coordinate system; a pointer from the node to the parent node; the cumulative cost g from the starting node to the node; a heuristic function h of node-to-end point estimation; search density ρ in the vicinity of the node; the heuristic function h of the search node is:

where s represents the current state of the vehicle,

is the state of the nth local end point, wherein

Is the next local end point corresponding to the current state,

for the kth local end point in the cyclic accumulation, h_2DRepresenting the estimation of the distance between two nodes by means of 2D A, without considering the kinematic constraint of the vehicle; h is_R-SShowing that the distance between two nodes is estimated by the Reeds-Shepp curve length without considering the obstacles in the environment.

According to OPEN table and BUFF tableDetermining the current node of the point: if the OPEN table and the BUFF table are both empty, the search is determined to be failed, and a feasible path from the starting point to the end point may not exist; if only the OPEN table is not empty, taking out the node of the minimum point of g + h in the OPEN table as the current node; if only the BUFF table is not empty, taking out the node with the minimum rho x h in the BUFF table as the current node; when both tables are not empty, respectively taking out the node s of the minimum point of g + h in the OPEN table_OPENAnd the node s with the minimum rho h in the BUFF table_BUFFWhen the node taken out from the BUFF table satisfies rho(s)_BUFF)＜ρ_thWhen it is, will s_BUFFAs the current node, and will s_OPENPutting back; otherwise, will s_OPENAs the current node, and will s_BUFFAnd (6) putting back.

In the implementation of the hybrid A method, the curvature change rate is specified to satisfy

And length of spiral element

And enabling the sub-node angle to accord with the discretization division result of the angle space, and performing off-line calculation on the motion element and storing the motion element in a lookup table form.

S308, new vehicle position (x)₁,y₁) Direction of θ₁Curvature kappa₁And old node vehicle position (x)₀,y₀) Direction of θ₀Curvature kappa₀The relationship of (1) is:

where σ is the rate of curvature change, L is the length of the primitive, and R (-) is a rotation matrix of 2 x 2;

for the case of 0, the primitive degenerates into a straight line or a circular arc, i.e., in the form of a primitive in a mixture A, with the expression

For σ ≠ 0, the correlation parameter can be obtained by fresnel integration calculation as follows:

wherein F_CAnd F_SIs Fresnel integral, satisfy

S400, constructing a complete path from the end point comprises the following steps:

s401, starting from a terminal node, generating a path sequence according to a father pointer relation until reaching a node corresponding to a starting point;

s402, reversing the node sequence generated in the S401, and extracting the position, orientation and curvature information of each step from the node sequence;

s403, interpolation is carried out according to the curve type to generate a more compact result.

Compared with the prior art, the invention has at least the following beneficial effects:

the invention provides a new hierarchical path planning scheme, which effectively improves the accuracy of a heuristic function under a fine scale by searching a guide path under a coarse scale, thereby reducing the time consumption of searching; in a specific searching method, by introducing a double-priority queue, the problem of repeated expansion is avoided; meanwhile, the generation of abnormal paths is avoided due to the orientation limitation; the search speed is further increased by the two methods; by introducing the expanded search space and the spiral search elements, the planned path has the characteristic of smooth curvature, so that the vehicle can follow more conveniently, and the invention can be applied to different parking and traffic control guiding scenes.

Drawings

Fig. 1 is a schematic view of the overall framework of the present invention.

FIG. 2 is a schematic diagram of the generation of a local endpoint.

Fig. 3 is a diagram of a spiral-based curve primitive.

Detailed Description

The present invention will be described in detail below with reference to the accompanying drawings.

The invention provides a rapid local path generation method facing a large-scale scene, which modifies a hybrid A method to complete rapid and safe local path planning under a large-scale complex scene, and greatly improves the search speed of the hybrid A method under the complex scene by introducing guide points obtained by primary global path planning, improved heuristic functions and a limited curve expansion mode. In addition, by introducing the expanded search space and the search elements of the spiral, the planned path has the characteristic of smooth curvature, so that the vehicle can follow more conveniently, and the invention can be applied to different parking and traffic control guiding scenes.

The flow chart of the method provided by the invention is shown in fig. 1 and is divided into two processes, wherein the first process is to generate a guide path based on an a-method in an overall 2D grid diagram, and divide the guide path and generate a local terminal point. The second process is to generate a trajectory to the end point based on a modified hybrid a method under guidance of the local end point. In the second part of the searching process, the searching process is enabled to have higher speed through a layered heuristic function, double searching queues and orientation consistency-based limitation; meanwhile, by introducing the search elements with continuous curvature, the curvature continuity of the final generated path can be realized, and the method can be better applied to the vehicle tracking process.

The method comprises the following specific steps:

step 1: generating a local grid map; the method comprises the following steps:

and 1.1, loading a pre-stored global map, and cutting a sub-map in a vehicle body coordinate system according to the current positioning result of the vehicle. Based on the position and the orientation of a pre-stored global map anchor point in a world coordinate system (such as UTM) and the position and the orientation of the current position of the vehicle output by a positioning module in the same world coordinate system, a local grid map with the current position of the vehicle as the center is obtained by rotating and cutting the global map.

And 1.2, removing the point cloud on the ground in the laser radar point cloud according to the height. The points in the set point cloud may be represented as P { (x) in the vehicle coordinate system_i,y_i,z_i) Get rid of it, then through setting up the height threshold z_thAnd reserving a non-ground point set P' { (x) in the laser radar point cloud_i,y_i,z_i)|z_i＜z_th}。

Step 1.3, a local occupancy grid map is constructed, representing the situation of obstacles around the vehicle. And marking the corresponding position of the non-ground point corresponding to the laser radar point cloud in the grid map as an obstacle. As an example, let the coordinate of the center of the upper left corner of the image under the coordinate system of the vehicle body be (X)₀,Y₀) The horizontal direction of the image is parallel to the orientation of the vehicle body, and the corresponding pixel resolution is r; if a non-ground point in the point cloud is (x, y), the pixel coordinate (row, column) in the local grid image is

Step 1.4, according to task requirements, marking the known static obstacle boundary as an occupied grid in a grid map.

And step 1.5, performing expansion processing on the grid map according to the number of pixels corresponding to half of the width of the vehicle so as to avoid the safety problem caused by the measurement error of the sensor.

Step 2: generating a global guide path and extracting key points (namely channel points); the execution process of this step is shown in fig. 2, and specifically includes the following steps:

step 2.1, mapping the terminal point of the vehicle local planning in the world coordinate system to a certain pixel position in the local grid map;

step 2.2, the algorithm A is applied to generate a guide path P from the current position of the vehicle to the terminal_rawAs shown in fig. 2 (a). The A-star algorithm adopts search expansion of adjacent grids towards 8 directions aroundAnd (4) after a complete guide path is obtained, taking a point which is less than the length of the vehicle from the obstacle as a candidate point, and recording the guide path as P_rawAs shown in fig. 2 (b).

Step 2.3, analyze the guide path P_rawThe neighborhood corresponding to each candidate point in the map is constructed with rays in eight directions until the rays intersect with the obstacle in the local grid map, as shown in fig. 2 (c). The complete guide path P is considered when the sum of the lengths of the two oppositely oriented rays is smaller than a threshold value D_rawAt this point in the narrow passage. Taking out the points in the middle of all the points in the narrow passage to form a set P.

And 2.4, refining the orientation and the channel width of each point in the set P. For the adjacent first orientation and second orientation, the first detection ray and the second detection ray are calculated through trigonometric relation to obtain refined channel orientation and half-boundary distance; as an example, for two adjacent orientations θ₁And theta₂Length d of₁And d₂As shown in fig. 2 (d). Refined channel orientation and half-boundary distance can be obtained through trigonometry

And 2.5, moving each point in the P according to the direction and the boundary distance of the channel obtained by refinement to enable each point to be positioned at the center of the channel in the vertical direction. A guidance end point is generated from each channel point, and the position, orientation, and channel width of each guidance end point are recorded, as shown in fig. 2 (e).

And step 3: implementing a modified hybrid a algorithm; the method comprises the following specific steps:

step 3.1 defines the structure of the search node:

firstly, the position, the orientation and the track curvature of a vehicle reference point under a local coordinate system; pointer from node to father node; cumulative cost g from the starting point node to the node; a heuristic function h from the node to the end point estimation; searching density rho near the node.

Step 3.2 heuristic function h of searching node is

Where s represents the current state of the vehicle,

is the state of the nth local end point, wherein

Is the next local end point corresponding to the current state,

is the kth local end point in the loop accumulation. H in the formula_2DRepresenting the estimation of the distance between two nodes by means of 2D A, without considering the kinematic constraint of the vehicle; h is_R-SShowing that the distance between two nodes is estimated by the Reeds-Shepp curve length without considering the obstacles in the environment.

The heuristic function estimation performed according to the above equation has the following characteristics:

1. when the local end points are the same, the heuristic function variation is not affected by the environment behind the local end points. And the environment from the current state to the local terminal is simpler, h_2DAnd h_R-SThe actual estimation of the local end point cost from the current state is more accurate. The accuracy of the heuristic function and the searching optimality are both related to the number of searching nodes, and the method can reduce the number of invalid searching nodes in the algorithm searching process and realize quick searching. 2. When the vehicle passes through the local terminal, the variation of the heuristic function is larger than the cost of the actual motion of the vehicle, so that the algorithm tends to search for the next local terminal, and the terminal is touched macroscopically more quickly; can make the current search facing local end pointThe method is not influenced by subsequent complex environments, so that more accurate heuristic function estimation is realized; the multiplication factor of the subsequent end point allows a larger gradient effect across the local end points.

Step 3.3 search for node s₀The proximity search density of

Wherein s is the node s searched for with the current₀All other search nodes with a distance less than 2.ρ(s)₀) Reflected in the current node s₀The number of other search nodes in the vicinity. This data is not stored with the node, but is stored statically in the global map, and the current new node neighborhood is updated as new nodes are generated. The invention rearranges the order of expansion of the nodes by this data (see 3.5).

3.4, constructing an OPEN table for storing nodes to be explored; and the BUFF table is used for temporarily storing the nodes needing to be sorted according to the density. And only installing the nodes corresponding to the current position of the vehicle in the OPEN table, and leaving a BUFF table empty. Setting an initial density discrimination threshold rho_th＝0。

Step 3.5, if the OPEN table and the BUFF table are both empty, the search is judged to be failed, and a feasible path from the starting point to the end point may not exist; if only the OPEN table is not empty, taking out the node of the minimum point of g + h in the OPEN table as the current node; if only the BUFF table is not empty, taking out the node with the minimum rho x h in the BUFF table as the current node; when both tables are not empty, respectively taking out the node s of the minimum point of g + h in the OPEN table_OPENAnd the node s with the minimum rho h in the BUFF table_BUFFWhen the node taken out from the BUFF table satisfies rho(s)_BUFF)＜ρ_thWhen it is, will s_BUFFAs the current node, and will s_OPENPutting back; otherwise, will s_OPENAs the current node, and will s_BUFFAnd (6) putting back.

Step 3.6 according to

Density of updateThreshold value rho_thEnsuring that it is close to the overall density average. In the formula

Representing p in the ith iteration_thThe value of the one or more of the one,

represents ρ in the i +1 th iteration_thThe value is obtained.

Step 3.7 if the current node is from the OPEN table, and ρ(s)_BUFF)＞ρ_thIt is loaded into the BUFF table and jumps to step 3.5. This means that the retrieval of a new node requires, in addition to checking the cost of the node and the heuristic functions, to ensure that the vicinity of the node is not over-explored. Through the operation, after the vicinity of one node is visited for multiple times, the access priority of the node is correspondingly reduced, namely, the system can preferentially explore unexplored areas to ensure timely escaping.

And 3.8, judging whether the current node meets the requirement of the global end point, if so, jumping to the step 4.1 to construct a path, and if not, continuing the searching process of the step 3.9.

Step 3.9 determines whether the current node can be linked to the next local end point via the RS curve, if so, such RS curve is created and a parent node relationship is constructed, the next local end point is added to the OPEN table and the process jumps to step 3.5.

And 3.10, performing child node expansion on the current node. The expansion process simultaneously considers the position, orientation and curvature of the nodes. The motion elements which are expanded and adopted by the integration of the vehicle kinematic equation are straight lines, circles or spirals. New vehicle position (x)_b,y_b) Direction of θ_bCurvature kappa_bAnd old node vehicle position (x)_a,y_a) Direction of θ_aCurvature kappa_aThe relationship of (1) is:

where σ is the rate of curvature change, L is the length of the primitive, and R (. cndot.) is a rotation matrix of 2 x 2.

wherein F_CAnd F_SIs Fresnel integral, satisfy

The position, orientation and curvature continuity of the inside of the motion element and the joint point can be ensured through the structure.

The effect of the spiral primitive-based transition is shown in fig. 3. Fig. 3(a) shows a spiral connecting a straight line and a circular arc, and fig. 3(b) shows a spiral connecting two circular arcs having different curvatures. Fig. 3(c) is a direct connection of the straight line and the circle used in the mixture a, for comparison. Fig. 3(d), 3(e) and 3(f) show the orientation change in different cases, and fig. 3(g), 3(h) and 3(i) show the curvature change in different cases. It can be seen that although both solutions have continuity of orientation, the curvature of the original mix a is abrupt, giving rise to the discomfort of the ride. In contrast, the scheme of the present invention generates a path with higher smoothness.

Further, by specifying the curvature change rate satisfies

And length of spiral element

The method can enable the sub-node angle to still conform to the discretization division result of the angle space, thereby calculating the motion element off line and storing the motion element in a lookup table form without influencing the searching performance in online operation.

3.11, generating position and orientation information of the child nodes according to the type of the expansion curve; and determining the g value of the child node according to the length of the expansion curve and the weighting coefficient, wherein the g value of the child node is equal to the g value of the parent node plus the weighting coefficient multiplied by the length of the expansion curve.

When the expansion curve is not straight, is a reversing path, or the expansion curve causes the orientation of the child node to be out of the orientation range, the weighting coefficient is additionally increased. The limitation of the orientation is mainly based on that the change of the orientation of the head of the vehicle is often a monotonous process in the driving process of a human driver, so that the condition of the orientation swing is cut off as an abnormal expansion result in advance.

And 3.12, calculating a heuristic function value from the child node to the end point according to the step 3.2, and storing the heuristic function value as a node h value.

Step 3.13 judges whether the child node is marked as a CLOSED state. If so, judging whether g + h of the existing node is larger than g + h of the child node or not. If the child node is not marked as a CLOSED state or g + h is less than g + h of the existing node, the child node is placed in the OPEN table and goes to 3.5 to repeat the search process. Step 3.12 is the same as step 3.13 as conventional a and blend a.

And 4, constructing a complete path from the end point, and sending the complete path to a control mechanism for execution.

Further, step 4 specifically includes the following steps:

and 4.1, starting from the terminal node, generating a path sequence according to the parent pointer relation until reaching the node corresponding to the starting point.

And 4.2, reversing the node sequence generated in the step 4.1, and extracting the position, orientation and curvature information of each step from the node sequence.

And 4.3, carrying out interpolation according to the curve type to generate a finer result.

The method of the invention is implemented on an unmanned test vehicle and integrated with a perception system based on a laser radar and a path tracking system based on a quintic spline. Experimental results show that the method can realize better path planning for scenes such as backing-up and warehousing, parking at the side position, head dropping in a narrow road and the like.

The invention provides a local path planning system of an autonomous driving vehicle, which comprises an obstacle module, a key point extraction module and a curve generation module based on a mixed A-x algorithm;

the obstacle module is used for acquiring a local grid map with the current position of the vehicle as the center and marking obstacles in the local grid map;

the key point extraction module is used for generating a global guide path and extracting key points;

a hybrid a algorithm based curve generation module is used to implement a modified hybrid a algorithm,

defining a structure of a search node, obtaining a heuristic function h and an adjacent search density of the search node based on the structure, and rearranging an expansion sequence of the node according to the adjacent search density; an OPEN table and a BUFF table are built, the OPEN table is used for storing nodes to be explored, and the BUFF table is used for temporarily storing the nodes needing to be sorted according to the density; setting initial density discrimination threshold rho at the same time_thWhen the current position of the vehicle is equal to 0, only the node corresponding to the current position of the vehicle is loaded in the OPEN table, and the BUFF table is left empty; determining a current node according to points in the OPEN table and the BUFF table; according to

Updating the density threshold ρ_th(ii) a If the current node is from the OPEN table, and ρ(s)_BUFF)＞ρ_thIf yes, loading the node into a BUFF table, and determining a current node according to the points in the OPEN table and the BUFF table; judging whether the current node meets the requirement of the global end point, and if the current node meets the requirement of the global end point, sending a result to a path generation module; if the current node does not meet the requirement of the global end point, judging whether the current node can be linked to the next local end point through an RS curve, and if so, creating the local end pointAn RS curve of the sample is established, a father node relation is established, the next local terminal is added to an OPEN table, and a current node is determined according to points in the OPEN table and the BUFF table; performing child node expansion on the current node to obtain an expansion curve; specifically, the motion primitive obtained and expanded by the vehicle kinematic equation integration is a straight line, a circle or a spiral and corresponds to a new vehicle position (x)₁,y₁) Direction of θ₁Curvature kappa₁And old node vehicle position (x)₀,y₀) Direction of θ₀Curvature kappa₀Based on the spiral primitive transition, the line connects the straight line and the circular arc; generating position and orientation information of the child nodes according to the type of the expansion curve; calculating heuristic function values from the sub-nodes to the end point according to the position and orientation information, storing the heuristic function values as node h values, updating an OPEN table by adopting a traditional A and mixed A method, and determining the current node according to the points in the OPEN table and the BUFF table;

and the path generation module is used for constructing a complete path from the end point and sending the complete path to the vehicle orientation and speed controller and the vehicle actuator.

The invention can also provide an autonomous driving vehicle system, which comprises a data layer, a planning layer and an execution layer, wherein the data layer stores static obstacle data, and is provided with a sensing and object detection module for acquiring data based on sensor sensing; the execution layer comprises a vehicle self-moving system, a direction and speed controller and a vehicle actuator, and the planning layer plans a path based on the information of the data layer, converts the planned path into a control command and sends the control command to the execution layer.

The invention can also provide a computer device, which comprises a processor and a memory, wherein the memory is used for storing the computer executable program, the processor reads part or all of the computer executable program from the memory and executes the computer executable program, and when the processor executes part or all of the computer executable program, the hierarchical local path planning method applicable to large-scale complex scenes can be realized.

In another aspect, the present invention provides a computer-readable storage medium, in which a computer program is stored, and when the computer program is executed by a processor, the hierarchical local path planning method applicable to a large-scale complex scene according to the present invention can be implemented.

The computer equipment can adopt an onboard computer, a notebook computer, a desktop computer or a workstation.

The processor may be a Central Processing Unit (CPU), a Digital Signal Processor (DSP), an Application Specific Integrated Circuit (ASIC), or an off-the-shelf programmable gate array (FPGA).

The memory can be an internal storage unit of a vehicle-mounted computer, a notebook computer, a desktop computer or a workstation, such as a memory and a hard disk; external memory units such as removable hard disks, flash memory cards may also be used.

Computer-readable storage media may include computer storage media and communication media. Computer storage media includes volatile and nonvolatile, removable and non-removable media implemented in any method or technology for storage of information such as computer readable instructions, data structures, program modules or other data. The computer-readable storage medium may include: a Read Only Memory (ROM), a Random Access Memory (RAM), a Solid State Drive (SSD), or an optical disc. The Random Access Memory may include a resistive Random Access Memory (ReRAM) and a Dynamic Random Access Memory (DRAM).

Claims

1. A hierarchical local path planning method suitable for a large complex scene is characterized by comprising the following steps:

s200, generating a global guide path and extracting key points;

s304, according to

Updating the density threshold ρ_th；

S305, if the current node is from the OPEN table, and ρ (S)_BUFF)>ρ_thIf yes, the current node is loaded into the BUFF table, and then the step is skipped to S303;

2. The hierarchical local path planning method applied to the large-scale complex scene according to claim 1, wherein a local grid map with a vehicle current position as a center is obtained: and obtaining a local grid map taking the current position of the vehicle as the center by rotating and cutting the global map based on the position and the orientation of the anchor point of the pre-stored global map in the world coordinate system and the position and the orientation of the current position of the vehicle output by the positioning module in the same world coordinate system.

3. The hierarchical local path planning method applicable to large-scale complex scenes according to claim 1, wherein obstacles are marked in the local grid map: setting a height threshold value in a local grid map, and removing a ground point set in laser radar point cloud of the local grid map according to the height threshold value to obtain a non-ground point set in the laser radar point cloud; marking the corresponding position of the non-ground point set in the local grid map as an obstacle.

4. The method for planning the hierarchical local path suitable for the large complex scene according to claim 1, wherein the step of generating the global guiding path and extracting the key points in S200 specifically comprises the following steps:

5. The hierarchical local path planning method applied to large-scale complex scenes according to claim 4, characterized in that an A-x algorithm is applied to generate a guide path P from the current position of the vehicle to the end point_rawIn the process, after a complete guide path is obtained by adopting search expansion of 8 nearby grids in the peripheral direction based on an A-x algorithm, points which are far away from an obstacle and are smaller than the length of a vehicle are used as candidate points, and the guide path is marked as P_raw(ii) a When the orientation and the channel width of each point in the set P are refined, for the adjacent first orientation and second orientation, the first detection ray and the second detection ray are calculated through trigonometric relation to obtain the refined channel orientation and half-boundary distance.

6. The hierarchical local path planning method applicable to large-scale complex scenes according to claim 1, wherein the structure of the search node is specifically as follows: the position, the orientation and the track curvature of a vehicle reference point under a local coordinate system; a pointer from the node to the parent node; the cumulative cost g from the starting node to the node; a heuristic function h of node-to-end point estimation; search density ρ in the vicinity of the node; the heuristic function h of the search node is:

where s represents the current state of the vehicle,

is the state of the nth local end point, wherein

Is at presentThe next local end point to which the state corresponds,

7. The hierarchical local path planning method applied to large-scale complex scenarios according to claim 1, wherein when determining the current node according to the points in the OPEN table and the BUFF table: if the OPEN table and the BUFF table are both empty, the search is determined to be failed, and a feasible path from the starting point to the end point may not exist; if only the OPEN table is not empty, taking out the node of the minimum point of g + h in the OPEN table as the current node; if only the BUFF table is not empty, taking out the node with the minimum rho x h in the BUFF table as the current node; when both tables are not empty, respectively taking out the node s of the minimum point of g + h in the OPEN table_OPENAnd the node s with the minimum rho h in the BUFF table_BUFFWhen the node taken out from the BUFF table satisfies rho(s)_BUFF)＜ρ_thWhen it is, will s_BUFFAs the current node, and will s_OPENPutting back; otherwise, will s_OPENAs the current node, and will s_BUFFAnd (6) putting back.

8. The hierarchical local path planning method applied to large-scale complex scenes according to claim 1, wherein in the implementation of the hybrid A method, the curvature change rate is specified to satisfy

And length of spiral element

9. The method for hierarchical local path planning applicable to large-scale complex scenarios according to claim 1, wherein S308 is a new vehicle position (x)₁，y₁) Direction of θ₁Curvature kappa₁And old node vehicle position (x)₀，y₀) Direction of θ₀Curvature kappa₀The relationship of (1) is:

wherein F_CAnd F_SIs Fresnel integral, satisfy

10. The method for hierarchical local path planning applicable to large-scale complex scenarios according to claim 1, wherein S400, constructing a complete path from an end point includes the following steps: