Space-time reference trajectory generation method and system based on space-time hybrid ARA

Abstract

The invention discloses a space-time reference trajectory generation method and system based on space-time hybrid ARA *, belongs to the technical field of path trajectory generation, and solves the problem that a traditional path generation method cannot adapt to a complex urban traffic environment. Comprising the following steps: S1, constructing a space-time expansion state space, defining a control variable of a node, processing the control variable of the node, and performing constraint check on the node; s2, constructing a space-time hybrid ARA * algorithm, and outputting an optimal space-time reference trajectory based on the nodes after constraint check and the space-time hybrid ARA * algorithm; and S3, constructing road and obstacle boundary constraints based on the optimal space-time reference trajectory, and outputting a final trajectory.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of path trajectory generation, in particular to a spatiotemporal reference trajectory generation method and system based on a spatiotemporal hybrid ARA*. BACKGROUND

[0002] With the development of autonomous driving technology, efficient, accurate and safe trajectory planning methods have gradually become a research hotspot. Although the traditional two-dimensional hybrid A* algorithm can generate a preliminary path, it does not consider the time and speed dimensions, and the real-time performance and trajectory quality in dynamic scenes are greatly limited. Especially in the complex urban road, dynamic obstacles change frequently.

[0003] Therefore, the traditional method has the problems of insufficient real-time performance, planning trajectory difficult to meet the kinematic constraint conditions, low calculation efficiency and the like. Therefore, there is an urgent need for a spatiotemporal trajectory planning method that can balance real-time performance and path quality to adapt to complex urban traffic environments. SUMMARY

[0004] The present application solves the problem that the traditional path generation method cannot adapt to complex urban traffic environments.

[0005] The spatiotemporal reference trajectory generation method based on a spatiotemporal hybrid ARA* according to the present application comprises the following steps:

[0006] Step S1, constructing a spatiotemporal extended state space and defining the control variables of the nodes, processing the control variables of the nodes, and then performing constraint checking on the nodes;

[0007] Step S2, constructing a spatiotemporal hybrid ARA* algorithm, outputting an optimal spatiotemporal reference trajectory based on the nodes after constraint checking and the spatiotemporal hybrid ARA* algorithm;

[0008] Step S3, constructing road and obstacle boundary constraints based on the optimal spatiotemporal reference trajectory, and outputting a final trajectory.

[0009] Further, in an embodiment of the present application, in step S1, the construction of the spatiotemporal extended state space is specifically:

[0010] The original state space {s, l, theta} is expanded to form a spatiotemporal extended state space {s, l, theta, v, t}.

[0011] Further, in an embodiment of the present application, in step S1, the processing of the control variables of the nodes is specifically:

[0012] The control variables of the nodes are discretely sampled, and the nodes are expanded based on a kinematic model.

[0013] Furthermore, in one embodiment of the present invention, in step S1, the constraint check on the node is performed as follows:

[0014] Perform speed, collision and road boundary collision constraint checks on nodes respectively;

[0015] In terms of collision, the projection concept in the AABB model is used to initially screen obstacle nodes with collision risks, and then the GJK algorithm is used for collision detection;

[0016] In terms of road boundary collision, the CER model is adopted.

[0017] Furthermore, in one embodiment of the present invention, in step S2, outputting the optimal spatiotemporal reference trajectory based on the nodes after constraint checking and the spatiotemporal hybrid ARA* algorithm includes the following steps:

[0018] Step S201, respectively initialize the open list, the closed list and the inconsistent list, and set the initial heuristic function;

[0019] Step S202, performing a single search based on the nodes after constraint checking, and outputting the current suboptimal spatiotemporal reference trajectory;

[0020] Step S203: After updating the initial heuristic function, merge the inconsistent lists into the open list and rearrange them;

[0021] Step S204, determining whether the termination condition is met, if so, outputting the optimal spatiotemporal reference trajectory, if not, returning to step S202.

[0022] Furthermore, in one embodiment of the present invention, in step S201, the inconsistency list is used to store inconsistent nodes that are not in the open list.

[0023] Furthermore, in one embodiment of the present invention, in step S204, the termination condition is specifically:

[0024] The initial heuristic function is equal to 1 or the preset search time is reached.

[0025] Furthermore, in one embodiment of the present invention, in step S3, constructing road and obstacle boundary constraints based on the optimal spatiotemporal reference trajectory and outputting the final trajectory include the following steps:

[0026] Step S301: Select the vehicle position based on the optimal spatiotemporal reference trajectory, use a disk to cover the vehicle's rectangular outline, construct a half-plane whose normal vector points to the interior of the lane, and determine the positional relationship between the vehicle and the road boundary based on the half-plane;

[0027] Step S302: The half-plane with the normal vector pointing to the inside of the lane constructed in step S301 is translated along the normal vector direction to determine the positional relationship between the vehicle and the road boundary again;

[0028] Step S303: Determine the edge of the obstacle closest to the vehicle. Based on this edge, obtain two endpoints and construct a trapezoid to describe the obstacle's spatial position. Use a circular disk to cover the vehicle's rectangular outline, construct a half-plane whose normal vector points into the lane, and determine the positional relationship between the vehicle and the obstacle based on the half-plane.

[0029] Step S304: The half-plane with the normal vector pointing to the inside of the lane constructed in step S303 is translated along the normal vector direction to determine the positional relationship between the vehicle and the obstacle again;

[0030] In step S305 , based on the positional relationship between the vehicle and the road boundary determined in step S302 and the positional relationship between the vehicle and the obstacle determined in step S304 , a final trajectory is output.

[0031] The spatiotemporal reference trajectory generation system based on spatiotemporal hybrid ARA* described in the present invention includes the following modules:

[0032] Module S1 constructs the spatiotemporal extended state space and defines the control variables of the nodes. After processing the control variables of the nodes, the constraints of the nodes are checked.

[0033] Module S2, constructs the spatiotemporal hybrid ARA* algorithm, and outputs the optimal spatiotemporal reference trajectory based on the nodes after constraint checking and the spatiotemporal hybrid ARA* algorithm;

[0034] Module S3 constructs road and obstacle boundary constraints based on the optimal spatiotemporal reference trajectory and outputs the final trajectory.

[0035] The present invention solves the problem that traditional path generation methods cannot adapt to complex urban traffic environments.

[0036] Specific beneficial effects include:

[0037] 1. The present invention describes a method for generating spatiotemporal reference trajectories based on spatiotemporal hybrid ARA*. Conventional path generation methods are unable to adapt to complex urban traffic environments. To address these technical issues, the present invention adds speed and time dimensions to the original state space, forming an extended state space, thereby resolving the technical issues of conventional technologies.

[0038] 2. The spatiotemporal reference trajectory generation method based on spatiotemporal hybrid ARA* described in this invention, compared to traditional single-stage fine collision detection methods, adopts a combined strategy of the AABB method and the GJK algorithm in the node expansion phase. Through a hierarchical detection approach of "coarse screening first, then fine detection", this method significantly reduces the computational complexity of fine collision detection and effectively improves the overall collision detection efficiency. It is particularly suitable for complex and dynamic traffic scenarios.

[0039] 3. The spatiotemporal reference trajectory generation method based on spatiotemporal hybrid ARA* described in this invention innovatively proposes an adaptive ARA* algorithm, compared to traditional two-dimensional hybrid A* algorithms or fixed-weight heuristic search methods. By gradually reducing the heuristic search coefficient ∈ and introducing an inconsistent node list, this algorithm effectively reduces the computational complexity of redundant node expansion, achieves a dynamic balance between trajectory search efficiency and quality, significantly improves the algorithm's real-time performance and path accuracy, reduces the need for manual parameter adjustment, and enhances the algorithm's generalization capability.

[0040] 4. The spatiotemporal reference trajectory generation method based on spatiotemporal hybrid ARA* described in this invention combines the concepts of reachable sets and bounding boxes when constructing boundary constraints, compared to traditional boundary constraint methods that simply use bounding boxes (AABB, OBB) or single reachable set methods. Based on the spatiotemporal reference trajectory, it constructs semi-plane fine boundaries and obstacle trapezoidal space constraints. This effectively avoids the problems of excessively large or overly conservative boundary constraint spaces in traditional methods, improves the accuracy of boundary constraint descriptions, and significantly enhances the security of trajectory planning.

[0041] The spatiotemporal reference trajectory generation method based on spatiotemporal hybrid ARA* described in the present invention can optionally use different heuristic function expansion strategies or other collision detection methods, but this will reduce the planning real-time performance or trajectory accuracy to a certain extent. BRIEF DESCRIPTION OF THE DRAWINGS

[0042] The above and / or additional aspects and advantages of the present invention will become apparent and readily understood from the following description of the embodiments in conjunction with the accompanying drawings, in which:

[0043] Figure 1 is a flow chart of the spatiotemporal hybrid ARA* trajectory planning method described in Implementation Method 1;

[0044] Figure 2 This is a diagram of the vehicle model outline construction method described in the first embodiment;

[0045] Figure 3 is a schematic diagram of the boundary between the vehicle and the road according to the first embodiment;

[0046] Figure 4 Schematic diagram of the boundary between the vehicle and the obstacle according to the first embodiment. DETAILED DESCRIPTION

[0047] Various embodiments of the present application will be described in detail below with reference to the drawings. The embodiments described by reference to the drawings are exemplary and are intended to explain the present application, and are not to be understood as limiting the present application.

[0048] Embodiment one, the method for generating a spatiotemporal reference trajectory based on a spatiotemporal hybrid ARA*, comprising the following steps:

[0049] Step S1, constructing a spatiotemporal extended state space, and defining the control variables of the nodes, processing the control variables of the nodes, and performing constraint checking on the nodes;

[0050] Step S2, constructing a spatiotemporal hybrid ARA* algorithm, outputting an optimal spatiotemporal reference trajectory based on the nodes after constraint checking and the spatiotemporal hybrid ARA* algorithm;

[0051] Step S3, constructing a road and obstacle boundary constraint based on the optimal spatiotemporal reference trajectory, and outputting a final trajectory.

[0052] In the embodiment, in step S1, the construction of the spatiotemporal extended state space is specifically:

[0053] The original state space {s, l, θ} is extended to form a spatiotemporal extended state space {s, l, θ, v, t}.

[0054] In the embodiment, in step S1, the processing of the control variables of the nodes is specifically:

[0055] The control variables of the nodes are discretely sampled, and the nodes are expanded based on a kinematic model.

[0056] In the embodiment, in step S1, the constraint checking on the nodes is specifically:

[0057] The nodes are respectively subjected to constraint checking of speed, collision, and road boundary collision;

[0058] In terms of collision, the nodes of obstacles at risk of collision are preliminarily screened using the projection idea in the AABB model, and then subjected to collision detection using the GJK algorithm;

[0059] In terms of road boundary collision, the CER model is used.

[0060] In the embodiment, in step S2, the output of the optimal spatiotemporal reference trajectory based on the nodes after constraint checking and the spatiotemporal hybrid ARA* algorithm comprises the following steps:

[0061] Step S201, respectively initialize the open list, the closed list and the inconsistent list, and set the initial heuristic function;

[0062] Step S202, performing a single search based on the nodes after constraint checking, and outputting the current suboptimal spatiotemporal reference trajectory;

[0063] Step S203: After updating the initial heuristic function, merge the inconsistent lists into the open list and rearrange them;

[0064] Step S204, determining whether the termination condition is met, if so, outputting the optimal spatiotemporal reference trajectory, if not, returning to step S202.

[0065] In this embodiment, in step S201, the inconsistency list is used to store inconsistent nodes that are not in the open list.

[0066] In this embodiment, in step S204, the termination condition is specifically:

[0067] The initial heuristic function is equal to 1 or the preset search time is reached.

[0068] In this embodiment, in step S3, constructing road and obstacle boundary constraints based on the optimal spatiotemporal reference trajectory and outputting the final trajectory include the following steps:

[0069] Step S301: Select the vehicle position based on the optimal spatiotemporal reference trajectory, use a disk to cover the vehicle's rectangular outline, construct a half-plane whose normal vector points to the interior of the lane, and determine the positional relationship between the vehicle and the road boundary based on the half-plane;

[0070] Step S302: The half-plane with the normal vector pointing to the inside of the lane constructed in step S301 is translated along the normal vector direction to determine the positional relationship between the vehicle and the road boundary again;

[0071] Step S303: Determine the edge of the obstacle closest to the vehicle. Based on this edge, obtain two endpoints and construct a trapezoid to describe the obstacle's spatial position. Use a circular disk to cover the vehicle's rectangular outline, construct a half-plane whose normal vector points into the lane, and determine the positional relationship between the vehicle and the obstacle based on the half-plane.

[0072] Step S304: The half-plane with the normal vector pointing to the inside of the lane constructed in step S303 is translated along the normal vector direction to determine the positional relationship between the vehicle and the obstacle again;

[0073] In step S305 , based on the positional relationship between the vehicle and the road boundary determined in step S302 and the positional relationship between the vehicle and the obstacle determined in step S304 , a final trajectory is output.

[0074] Traditional methods, such as the two-dimensional hybrid A* algorithm, can generate preliminary paths but fail to consider the time and speed dimensions, significantly limiting real-time performance and trajectory quality in dynamic scenarios. This is particularly true in urban environments with complex roads and frequently changing dynamic obstacles.

[0075] In order to solve the above technical problems, Figure 1 As shown, this embodiment is based on the improvement of the two-dimensional hybrid A* algorithm and proposes a method for generating a spatiotemporal reference trajectory based on spatiotemporal hybrid ARA*, including the following steps:

[0076] Step S1, design of space-time node expansion scheme:

[0077] 1) In order to solve the problem that the traditional two-dimensional hybrid A* algorithm lacks speed and time dimensions and cannot effectively cope with dynamic environments, this implementation adds two dimensions, speed v and time t, to the original state space {s, l, θ} to form an extended state space {s, l, θ, v, t}. The control variables of the node are the front wheel angle δ and acceleration a. In order to match the actual movement ability of the vehicle and ensure that the spatial state between adjacent nodes can remain continuous, this method adopts a two-variable discretization method. The front wheel angle δ is in the interval [δ min ,δ max ] uniform discrete sampling, and to ensure that the zero angle can be included as a sampling point, the number of samples of δ should be set to an odd number; the acceleration a is determined according to the dynamic parameters of the vehicle in the interval [a min ,a max ], and then adopt non-uniform interval sampling with zero as the center and gradually increasing to achieve the dynamic step effect.

[0078] 2) Check the speed, collision and road boundary constraints of the nodes to ensure that each child node meets the vehicle kinematics, traffic regulations and safety requirements.

[0079] like Figure 2 As shown, first, we use the projection idea in the AABB model to quickly screen out obstacle nodes that may have collision risks. Specifically, when the current node time state is clear, the longitudinal position coordinates s of other traffic participants in the planning space are calculated. obs and the longitudinal position s of the node to be detected iA quick comparison is performed. If the longitudinal distance between the two exceeds the threshold, they are directly excluded; otherwise, they are retained. Secondly, the GJK (Gilbert-Johnson-Keerthi) algorithm is used to detect the fine outlines of obstacles that may cause collisions after the initial screening. Since the number of obstacle nodes to be detected has been greatly reduced in the initial screening stage, the algorithm's operating efficiency can be significantly improved while ensuring the accuracy of collision detection. In terms of road boundary collision detection, this paper adopts the CER model idea, which is to cover the vehicle's rectangular outline with several disks of the same radius. By projecting the center of the disk onto the road boundary, and then calculating whether the distance from the center to the road boundary is greater than the predetermined safety buffer distance, it can quickly and accurately determine whether the vehicle meets the road boundary constraints, thereby ensuring driving safety and planning efficiency.

[0080] Step S2, time-space hybrid ARA* algorithm design:

[0081] Although the spatiotemporal hybrid A* algorithm can generate a trajectory from the initial node to the target node, due to the high dimensionality of the planning space, it is difficult to meet real-time requirements in some complex scenarios. Therefore, this implementation further improves the spatiotemporal reference trajectory generation algorithm and improves the spatiotemporal hybrid A* algorithm to the spatiotemporal hybrid ARA* algorithm to improve search efficiency and trajectory quality.

[0082] In the A* algorithm framework, the heuristic function is responsible for estimating the minimum cost from the current node to the target node and guiding the search direction. This function must meet the consistency condition, that is, for any node N i and its successor node N child have:

[0083] H(N i )≤G(N i ,N child )+H(N child );

[0084] In the formula, G(N i ,N child ) represents the i to N child the actual cost.

[0085] Consistency ensures that the algorithm finds the optimal solution and avoids repeated node expansion. However, the weighted A* algorithm introduces an expansion factor ∈>1 in the heuristic function to speed up the search and reduce the number of node expansions, but this sacrifices the optimality of the solution and may deviate from the shortest path.

[0086] To strike a balance between efficiency and optimality, this implementation introduces the ARA* strategy: Initially, a large ∈ is set to quickly obtain an approximate solution. Subsequently, ∈ is gradually reduced over multiple iterations, eventually converging to 1, gradually approaching the optimal solution. After each round of ∈ reduction, the current search result is retained as a basis for further refinement in the next round of search, balancing computational efficiency and solution quality.

[0087] When the cost function of a node decreases, the cost relationship between the node and its successor nodes may no longer satisfy consistency. For example, when N p Yes N i The optimal predecessor node should satisfy the following conditions:

[0088]

[0089] In the formula, G(N i ) represents node N i The cost function value of G(N p ,N i 0 represents slave node N p To node N i The cost, pred(N i ) represents N i The set of all predecessor nodes. p The cost function value decreases if N p There is a local inconsistency between it and its successor node, which will cause:

[0090]

[0091] It is believed that N i In a partially inconsistent state.

[0092] From the definition of local inconsistency, it can be seen that the open list can be regarded as a set of nodes that need to propagate local inconsistencies: when the cost function value of a node decreases, it is added to the open list, and when the node is expanded, it is removed from the open list until its cost function value decreases again. When the ∈ value is greater than 1, the consistency of the heuristic function may be destroyed, and it cannot be ensured that each node is expanded only once during expansion, which will affect the validity of the final search results. If the restriction of expanding each node only once is simply added, then the open list will not be guaranteed to contain all inconsistent nodes. At this time, the open list is not complete because the information of all inconsistent nodes must be retained as the basis for subsequent iterative searches. Therefore, this embodiment adds an inconsistent list to the algorithm to store inconsistent nodes that are not in the open list. By combining the open list and the inconsistent list, all locally inconsistent nodes can be accurately stored and used as the starting point for inconsistency propagation in each new search.

[0093] Since the search process needs to make use of the past search results, the target node might never appear locally inconsistent, i.e. might not be put into the open list. In this case, the termination condition of the Spatiotemporal Hybrid A* algorithm will become invalid, causing the search process to possibly never terminate. To solve this problem, the termination condition in the Spatiotemporal Hybrid ARA* algorithm is set as: stop when the cost of the termination node is less than or equal to the minimum cost in the open list. Such a termination condition ensures that the target node will not be expanded, while other nodes with the same cost as the target node might also not be expanded.

[0094] In addition, since the algorithm needs to make full use of the existing search results, in some cases the target node N goal might not be added to the open list due to local inconsistency, and if the traditional "expand the target node" is continued to be used as the termination condition, the algorithm might not end normally. To solve this problem, the termination condition is redefined in the present embodiment: when the total cost G(N goal )+∈·H(N goal ) of the target node is less than or equal to the minimum total cost of all nodes in the open list, the algorithm terminates. This strategy not only avoids unnecessary expansion of the target node, but also improves the overall search efficiency.

[0095] The ARA* algorithm performs a complete search using the set initial ∈ in the initialization stage. Then, before each iteration, the inconsistent nodes are added to the open list, the node list is reordered, and the search process is performed again. After each search is completed, the heuristic coefficient is updated according to the current search results:

[0096]

[0097] Then ∈ is further reduced (such as ∈ = ∈ - Δ∈) to improve the search quality. When ∈ = 1, or the search time reaches the preset upper limit t total , the algorithm outputs the current optimal solution as the final trajectory.

[0098] Now the existing Spatiotemporal Hybrid A* algorithm is introduced:

[0099] Design of the spatiotemporal node heuristic function:

[0100] The node cost function of the Spatiotemporal Hybrid A* algorithm is composed of the cost function and the heuristic function, and is represented as:

[0101] F i =G i +H i ;

[0102] In the formula, F i represents the cost function value of the node, Gi Represents the cost function value of the node, H i The heuristic function value representing the node. The heuristic function estimates the minimum cost from the current node to the target node, guiding the search direction, effectively compressing the search range, and improving computational efficiency. Traditional hybrid A* algorithms often use A* search results and Reeds-Sheep curves to form a heuristic function. While this algorithm has some guidance capabilities, it is computationally expensive and time-consuming, making it unsuitable for dynamic traffic scenarios requiring high real-time performance. To address these technical issues, as described above, this embodiment proposes the ARA* algorithm.

[0103] This implementation proposes to use the Euclidean distance between the node and the target node in the Frenet coordinate system as the heuristic function:

[0104]

[0105] Where s goal With l goal Represent the vertical and horizontal positions of the target node respectively, w h Represents the weight coefficient of the heuristic term. This method not only ensures the search effectiveness, but also avoids the search direction misleading problem caused by large road curvature in traditional methods, significantly improving the real-time performance and accuracy of trajectory planning.

[0106] Design of spatiotemporal node cost function:

[0107] The node cost function is used to represent the actual cumulative cost from the starting node to the current node. To improve the overall performance of the trajectory planning results, this implementation incorporates four indicators, namely, traffic efficiency, deviation, smoothness, and comfort, into the cost function, forming the following cost increment expression:

[0108]

[0109] Wherein, the traffic efficiency cost reflects the difference between the actual speed and the expected speed:

[0110]

[0111] Where w efficient represents the traffic efficiency weight, v expected Represents the desired vehicle speed.

[0112] The deviation cost measures the lateral deviation of the vehicle from the lane centerline:

[0113]

[0114] Where w offset Represents the weight of the degree of deviation.

[0115] The smoothness cost is based on the change of longitudinal and lateral deviations between nodes, which is controlled by the front wheel angle δ i and acceleration a i The change of directly determines:

[0116]

[0117] Where, and Represent the weights of horizontal smoothness and vertical smoothness respectively.

[0118] The comfort cost is calculated based on longitudinal and lateral acceleration and jerk to ensure driving comfort:

[0119]

[0120] Where, and Represent the weights of longitudinal acceleration and lateral acceleration respectively, and Represent the weights of longitudinal acceleration and lateral acceleration respectively.

[0121] Time-space hybrid A* algorithm process:

[0122] Based on the aforementioned steps of spatiotemporal node expansion, cost function design, and heuristic function design, trajectory search is performed by maintaining an open list and a closed list given the target node.

[0123] Step S3: Design of road and obstacle boundary information matching method:

[0124] 1) First, analyze the road boundary. Select the specific position of the vehicle based on the spatiotemporal reference trajectory, and use three discs to cover the rectangular outline of the vehicle. Taking the circle at the end of the vehicle as an example, project the center of the circle onto the reference line to obtain the projection point and the corresponding road boundary point. Extending a certain distance forward and backward along the boundary, two extended boundary points R1 and R2 can be obtained. Based on R1 and R2, a half-plane with a normal vector pointing to the inside of the lane can be constructed. The half-plane is translated in the direction of the normal vector by the same distance as the radius of the covering circle. The translated half-plane is Figure 3 L1 and L2 shown in the figure form a half-plane. Assuming that the parameters corresponding to the half-plane represented by points L1 and L2 are {a, b, c}, the expression of the half-plane is:

[0125] ax+by+c=0;

[0126] Substituting the coordinates of the center of the circle covered by the vehicle into the equation, we can obtain:

[0127] ax0+by0+c;

[0128] The sign of the result of the above equation intuitively reflects the positional relationship between the vehicle and the road boundary. As mentioned earlier, the normal vector of the half-plane points inwards to the lane, so a result greater than 0 indicates that the vehicle is traveling in the safe zone on the inside of the road. A result less than 0 indicates that the vehicle has traveled outside the road and collided with the road boundary.

[0129] 2) In order to further improve the safety of vehicle driving, the original half-plane can be further translated along the normal vector direction to obtain a more conservative half-plane, thereby providing sufficient safety buffer space. Figure 3 The half plane formed by S1 and S2 is shown.

[0130] 3) For obstacles, first determine the relative position of the obstacle to the ego vehicle, determine the edge closest to the ego vehicle, offset this edge toward the ego vehicle by a certain distance, and simultaneously extend it forward and backward along the direction of the obstacle's movement to obtain two endpoints. Next, project the obstacle's position onto the road boundary and extend it forward and backward based on the projected point to obtain two points on the road boundary. By combining these four vertices, a trapezoid is constructed to describe the spatial position of the obstacle, as shown in the figure below: Figure 4 When constructing obstacle boundary constraints, the idea is similar to constructing road boundaries. Three circles are also used to completely cover the vehicle's rectangular frame, and the center of each circle is matched with a half-plane whose normal vector points to the inside of the lane. Figure 4 For example, the centers of the three circles covering the vehicle are marked as C1, C2 and C3 respectively. When the center C1 is projected to the road boundary, it will intersect with the upper base of the trapezoidal space. At this time, the half-plane L1 generated by translating the half-plane formed by the two endpoints of the upper base along the normal vector direction by a radius is the obstacle boundary corresponding to C1. When the center C2 is projected to the road boundary, it will intersect with the waist of the trapezoidal space. If the half-plane formed by the waist of the trapezoidal space is directly used as the obstacle boundary constraint, the solution space will be greatly compressed. Therefore, in this case, the half-plane L2 that coincides with the upper base is directly used as the corresponding obstacle boundary. When the center C3 is projected to the road boundary, it will not intersect with the trapezoidal space. At this time, the obstacle boundary is consistent with the road boundary and is represented by the half-plane L3.

[0131] 4) After obtaining the obstacle boundaries corresponding to the three circle centers, for the sake of driving safety, these half-planes are translated along the normal vector direction for a certain distance to obtain a more conservative half-plane. Figure 4 S1, S2 and S3 shown.

[0132] Based on the above road and obstacle boundary constraints, the final trajectory is output.

[0133] Embodiment 2: A spatiotemporal reference trajectory generation system based on spatiotemporal hybrid ARA* described in this embodiment includes the following modules:

[0134] Module S1 constructs the spatiotemporal extended state space and defines the control variables of the nodes. After processing the control variables of the nodes, the constraints of the nodes are checked.

[0135] Module S2, constructs the spatiotemporal hybrid ARA* algorithm, and outputs the optimal spatiotemporal reference trajectory based on the nodes after constraint checking and the spatiotemporal hybrid ARA* algorithm;

[0136] Module S3 constructs road and obstacle boundary constraints based on the optimal spatiotemporal reference trajectory and outputs the final trajectory.

[0137] The above is a detailed introduction to the method and system for generating spatiotemporal reference trajectories based on spatiotemporal hybrid ARA* proposed in the present invention. Specific examples are used herein to illustrate the principles and implementation methods of the present invention. The description of the above embodiments is only used to help understand the method and core ideas of the present invention. At the same time, for those skilled in the art, according to the ideas of the present invention, there may be changes in the specific implementation methods and application scopes. In summary, the content of this specification should not be understood as limiting the present invention.

Claims

1. A method for generating spatiotemporal reference trajectories based on spatiotemporal hybrid ARA*, characterized in that: The following steps are involved: Step S1, constructing a spatiotemporal extended state space and defining the control variables of the nodes, processing the control variables of the nodes, and then performing constraint checks on the nodes; Step S2, constructing a spatiotemporal hybrid ARA* algorithm, and outputting an optimal spatiotemporal reference trajectory based on the nodes after constraint checking and the spatiotemporal hybrid ARA* algorithm; Step S3: Construct road and obstacle boundary constraints based on the optimal spatiotemporal reference trajectory and output the final trajectory.

2. The method for generating spatiotemporal reference trajectories based on spatiotemporal hybrid ARA* according to claim 1, characterized in that: In the step S1, the construction of the spatiotemporal extended state space is specifically as follows: The original state space {s, l, θ} is expanded to form a spatiotemporal extended state space {s, l, θ, v, t}.

3. The method for generating spatiotemporal reference trajectories based on spatiotemporal hybrid ARA* according to claim 1, characterized in that: In step S1, the control variables of the nodes are processed as follows: The control variables of the nodes are discretely sampled and the nodes are expanded based on the kinematic model.

4. The method for generating spatiotemporal reference trajectories based on spatiotemporal hybrid ARA* according to claim 1, characterized in that: In step S1, the constraint check is performed on the node, specifically: Perform speed, collision and road boundary collision constraint checks on nodes respectively; In terms of collision, the projection concept in the AABB model is used to initially screen obstacle nodes with collision risks, and then the GJK algorithm is used for collision detection; In terms of road boundary collision, the CER model is adopted.

5. The method for generating spatiotemporal reference trajectories based on spatiotemporal hybrid ARA* according to claim 1, characterized in that: In step S2, the output of the optimal spatiotemporal reference trajectory based on the nodes after constraint checking and the spatiotemporal hybrid ARA* algorithm includes the following steps: Step S201, respectively initialize the open list, the closed list and the inconsistent list, and set the initial heuristic function; Step S202, performing a single search based on the nodes after constraint checking, and outputting the current suboptimal spatiotemporal reference trajectory; Step S203: After updating the initial heuristic function, merge the inconsistent lists into the open list and rearrange them; Step S204, determining whether the termination condition is met, if so, outputting the optimal spatiotemporal reference trajectory, if not, returning to step S202.

6. The method for generating spatiotemporal reference trajectories based on spatiotemporal hybrid ARA* according to claim 5, characterized in that: In step S201, the inconsistency list is used to store inconsistent nodes that are not in the open list.

7. The method for generating spatiotemporal reference trajectories based on spatiotemporal hybrid ARA* according to claim 5, characterized in that: In step S204, the termination condition is specifically: The initial heuristic function is equal to 1 or the preset search time is reached.

8. The method for generating spatiotemporal reference trajectories based on spatiotemporal hybrid ARA* according to claim 1, characterized in that: In step S3, the step of constructing road and obstacle boundary constraints based on the optimal spatiotemporal reference trajectory and outputting the final trajectory includes the following steps: Step S301: Select the vehicle position based on the optimal spatiotemporal reference trajectory, use a disk to cover the vehicle's rectangular outline, construct a half-plane whose normal vector points to the interior of the lane, and determine the positional relationship between the vehicle and the road boundary based on the half-plane; Step S302: The half-plane with the normal vector pointing to the inside of the lane constructed in step S301 is translated along the normal vector direction to determine the positional relationship between the vehicle and the road boundary again; Step S303: Determine the edge of the obstacle closest to the vehicle. Based on this edge, obtain two endpoints and construct a trapezoid to describe the obstacle's spatial position. Use a circular disk to cover the vehicle's rectangular outline, construct a half-plane whose normal vector points into the lane, and determine the positional relationship between the vehicle and the obstacle based on the half-plane. Step S304: The half-plane with the normal vector pointing to the inside of the lane constructed in step S303 is translated along the normal vector direction to determine the positional relationship between the vehicle and the obstacle again; In step S305 , based on the positional relationship between the vehicle and the road boundary determined in step S302 and the positional relationship between the vehicle and the obstacle determined in step S304 , a final trajectory is output.

9. A spatiotemporal reference trajectory generation system based on spatiotemporal hybrid ARA*, characterized by: Includes the following modules: Module S1 constructs the spatiotemporal extended state space and defines the control variables of the nodes. After processing the control variables of the nodes, the constraints of the nodes are checked. Module S2, constructs the spatiotemporal hybrid ARA* algorithm, and outputs the optimal spatiotemporal reference trajectory based on the nodes after constraint checking and the spatiotemporal hybrid ARA* algorithm; Module S3 constructs road and obstacle boundary constraints based on the optimal spatiotemporal reference trajectory and outputs the final trajectory.

Images