Unmanned vehicle decision trajectory hybrid search method and device, medium and product

Abstract

The invention discloses an unmanned vehicle decision trajectory mixed search method and device, a medium and a product, and relates to the field of unmanned vehicle trajectory planning, the method comprises the following steps: dividing a trajectory planning process into a plurality of short-term plans, and dividing each short-term plan into a plurality of sampling stages; the starting point of the short-term planning except the first short-term planning is the end point of the optimal decision trajectory of the previous short-term planning, and in each short-term planning, target reachable set sampling, candidate trajectory generation and candidate trajectory cost calculation of a sampling stage one by one are carried out; the short-term planning is carried out until target reachable set sampling, candidate trajectory generation and candidate trajectory cost calculation of the last sampling stage in the short-term planning are completed, and then an optimal decision trajectory of the short-term planning is determined according to each candidate trajectory and the cost of each candidate trajectory in the last sampling stage in the short-term planning; and finally, determining a final decision trajectory according to all the short-term planned optimal decision trajectories. According to the method and the device, efficient and flexible track search can be realized.

Description

Technical Field

[0001] The present application relates to the field of unmanned vehicle trajectory planning, and in particular to a hybrid search method, device, medium and product for unmanned vehicle decision trajectories. Background Art

[0002] Unmanned vehicles (UAVs) have become a research hotspot in recent years. Trajectory planning for UAVs is one of their key technologies, and its performance directly determines the success of their operation. Safety and real-time performance are two important indicators of trajectory planning. Safety requires that the UAV avoid collisions with obstacles in the environment while following the planned trajectory. A planned trajectory is considered consistently safe if a safe trajectory or stopping conditions always exist before the robot completes a previously planned trajectory. Real-time performance requires that the UAV generate a trajectory within a specified timeframe. Furthermore, the trajectory must satisfy complex constraints, such as lane line constraints, speed and acceleration constraints, and frequent lane changes in structured road scenarios.

[0003] There are two main types of commonly used deterministic trajectory search methods: A-Star and its variants, and dynamic programming. A-Star and its variants generally use the shortest distance constraint. When considering complex constraints, they struggle to maintain consistency in heuristic terms, potentially resulting in a large number of ineffective searches and reduced search efficiency. Dynamic programming relies on a reachable target state and performs a reverse search, making it unsuitable for unexpected road congestion and lacking flexibility. Summary of the Invention

[0004] The purpose of this application is to provide a hybrid search method, device, medium and product for unmanned vehicle decision trajectories, which can realize efficient and flexible trajectory search.

[0005] To achieve the above objectives, this application provides the following solutions:

[0006] In a first aspect, the present application provides a hybrid search method for decision trajectories of an unmanned vehicle, comprising:

[0007] In the current short-term plan, the number and duration of sampling phases are determined based on the target unmanned vehicle motion state corresponding to the starting point of the current short-term plan. The starting point of each short-term plan other than the first short-term plan is the end point of the optimal decision trajectory of the previous short-term plan.

[0008] The target reachable set of the current sampling phase is determined based on the duration of the sampling phase, the starting point of the current sampling phase, and the motion state of the target unmanned vehicle corresponding to the starting point. The starting points of the sampling phases other than the first sampling phase are the specific target points in the target reachable set of the previous sampling phase. The specific target points are the end points of the candidate trajectories in the previous sampling phase whose costs are less than a preset cost threshold.

[0009] Based on the target points in the reachable target set of the current sampling stage and the motion state of the target unmanned vehicle corresponding to each target point, the starting point of the current sampling stage and the motion state of the target unmanned vehicle corresponding to the starting point of the current sampling stage, the candidate trajectories and the costs of each candidate trajectory in the current sampling stage are determined; among which, the end point of each candidate trajectory in the current sampling stage is the target point in the reachable target set of the current sampling stage.

[0010] Repeat the above process until the determination of each candidate trajectory and the cost of each candidate trajectory in the last sampling stage of the current short-term plan is completed.

[0011] The optimal decision trajectory of the current short-term plan is determined based on the candidate trajectories and their costs in the last sampling stage of the current short-term plan.

[0012] Determine whether the current short-term plan is the last short-term plan. If it is, determine the final decision trajectory based on the optimal decision trajectory of all short-term plans.

[0013] In a second aspect, the present application provides a computer device comprising: a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor executes the computer program to implement the steps of the hybrid search method for unmanned vehicle decision trajectories described in the first aspect.

[0014] In a third aspect, the present application provides a computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, implements the steps of the hybrid search method for unmanned vehicle decision trajectories described in the first aspect.

[0015] In a fourth aspect, the present application provides a computer program product, comprising a computer program, which, when executed by a processor, implements the steps of the hybrid search method for unmanned vehicle decision trajectories described in the first aspect.

[0016] According to the specific embodiments provided in this application, this application has the following technical effects:

[0017] The present application provides a hybrid search method, device, medium and product for unmanned vehicle decision trajectories, which divides the path planning (search) process into multiple short-term plans. The starting point of each short-term plan except the first short-term plan is the end point of the optimal decision trajectory of the previous short-term plan. In each short-term plan, the number of sampling stages and the duration of the sampling stage are determined according to the target unmanned vehicle motion state corresponding to the starting point of the short-term plan; the target reachable set of the current sampling stage is determined according to the duration of the sampling stage, the starting point of the current sampling stage and the target unmanned vehicle motion state corresponding to the starting point; the candidate trajectories and the cost of each candidate trajectory in the current sampling stage are determined according to each target point in the target reachable set of the current sampling stage and the target unmanned vehicle motion state corresponding to each target point, the starting point of the current sampling stage and the target unmanned vehicle motion state corresponding to the starting point of the current sampling stage; the above process is repeated until the determination of each candidate trajectory and the cost of each candidate trajectory in the last sampling stage of the short-term plan is completed; the optimal decision trajectory of the short-term plan is determined according to each candidate trajectory and the cost of each candidate trajectory in the last sampling stage of the short-term plan; finally, the final decision trajectory is determined according to the optimal decision trajectories of all short-term plans. This application divides the trajectory search process (long-term planning) into several short-term plans by adopting the "short-term traversal search + long-term greedy search" method. It can perform trajectory search based on complex constraints (such as the motion state constraints and lane constraints of the target unmanned vehicle) in each short-term planning process. Except for the first short-term plan, each short-term plan is based on the optimal trajectory obtained by the previous short-term plan for trajectory search, which will not significantly increase the overall complexity of the trajectory search, ensuring the efficiency of the trajectory search, and the trajectory search process does not need to rely on pre-set target points, ensuring the flexibility of the trajectory search. BRIEF DESCRIPTION OF THE DRAWINGS

[0018] In order to more clearly illustrate the embodiments of the present application or the technical solutions in the prior art, the following briefly introduces the drawings required for use in the embodiments. Obviously, the drawings described below are only some embodiments of the present application. For ordinary technicians in this field, other drawings can be obtained based on these drawings without creative work.

[0019] Figure 1 A flow chart of a hybrid search method for unmanned vehicle decision trajectories according to an embodiment of the present application;

[0020] Figure 2 A schematic diagram of the long-term planning principle provided in one embodiment of the present application;

[0021] Figure 3 A schematic diagram of the short-term planning principle provided in one embodiment of the present application;

[0022] Figure 4A schematic diagram of the structure of a computer device provided in one embodiment of the present application. DETAILED DESCRIPTION

[0023] When faced with sudden road congestion, the two existing search methods generally directly return a solution failure and then rely on additional modules to solve alternative trajectories. This undoubtedly makes the trajectory planning process more cumbersome and is not conducive to functional expansion.

[0024] This application proposes a hybrid search strategy for autonomous vehicle decision trajectories to overcome the challenges of existing technologies. This method is primarily targeted at autonomous vehicle decision trajectory planning in complex, structured scenarios. Considering that structured scenarios are typically multi-lane, this approach is crucial.

[0025] The following will be combined with the drawings in the embodiments of this application to clearly and completely describe the technical solutions in the embodiments of this application. Obviously, the embodiments described are only part of the embodiments of this application, not all of the embodiments. Based on the embodiments in this application, all other embodiments obtained by ordinary technicians in this field without making creative efforts are within the scope of protection of this application.

[0026] In order to make the above-mentioned purposes, features and advantages of the present application more obvious and easy to understand, the present application is further described in detail below with reference to the accompanying drawings and specific implementation methods.

[0027] In an exemplary embodiment, Figure 1 As shown, a hybrid search method for decision trajectories of an unmanned vehicle is provided, comprising the following steps 101 to 106. Among them:

[0028] Step 101 : In the current short-term plan, the number of sampling phases and the duration of the sampling phases are determined according to the motion state of the target unmanned vehicle corresponding to the starting point of the current short-term plan.

[0029] Among them, when the current short-term plan is the first short-term plan, the starting point of the current short-term plan is the current position of the target unmanned vehicle, so that the target unmanned vehicle motion state corresponding to the starting point of the current short-term plan is the target unmanned vehicle motion state corresponding to the current position; the current position of the target unmanned vehicle and the motion state corresponding to the current position are obtained through the on-board sensors of the target unmanned vehicle. When the current short-term plan is the nth short-term plan, the starting point of the current short-term plan is the end point of the optimal decision trajectory of the previous short-term plan, so that the target unmanned vehicle motion state corresponding to the starting point of the current short-term plan is the target unmanned vehicle motion state corresponding to the end point of the optimal decision trajectory of the previous short-term plan; 2≤n≤the number of short-term plans. The number of short-term plans is pre-set.

[0030] Step 102: Determine the reachable target set for the current sampling phase based on the duration of the sampling phase, the starting point of the current sampling phase, and the motion state of the target unmanned vehicle corresponding to the starting point. The starting points of the sampling phases other than the first sampling phase are the specific target points in the reachable target set of the previous sampling phase. The specific target points are the end points of the candidate trajectories in the previous sampling phase whose costs are less than a preset cost threshold.

[0031] That is to say, when the current sampling stage is the mth sampling stage in the current short-term plan (2≤m≤the number of sampling stages), the starting point of the current sampling stage is the specific target points in the target reachable set of the previous sampling stage, so that the target unmanned vehicle motion state corresponding to the starting point of the current sampling stage is the target unmanned vehicle motion state corresponding to the specific target points in the target reachable set of the previous sampling stage.

[0032] When the current sampling stage is the first sampling stage in the current short-term plan, the starting point of the current sampling stage is the starting point of the current short-term plan, so that the target unmanned vehicle motion state corresponding to the starting point of the current sampling stage is the target unmanned vehicle motion state corresponding to the starting point of the current short-term plan.

[0033] Step 103: Determine each candidate trajectory and its cost in the current sampling phase based on each target point in the reachable target set of the current sampling phase and the motion state of the target unmanned vehicle corresponding to each target point, the starting point of the current sampling phase and the motion state of the target unmanned vehicle corresponding to the starting point of the current sampling phase; wherein the end point of each candidate trajectory in the current sampling phase is the target point in the reachable target set of the current sampling phase.

[0034] The starting point of each candidate trajectory in the current sampling stage is the starting point of the current sampling stage.

[0035] Step 104 : Repeat steps 101 to 103 until the determination of each candidate trajectory and the cost of each candidate trajectory in the last sampling stage of the current short-term plan is completed.

[0036] Step 105 : Determine the optimal decision trajectory of the current short-term plan based on the candidate trajectories and their costs in the last sampling phase of the current short-term plan.

[0037] Step 106 , determining whether the current short-term plan is the last short-term plan, and if so, determining the final decision trajectory based on the optimal decision trajectory of all short-term plans.

[0038] The trajectory search method proposed in this application can be summarized as consisting of short-term planning and long-term planning, such as Figure 2 As shown. Long-term planning is carried out by N iThe first short-term plan (or short-term planning cycle) takes the current state of the target unmanned vehicle (hereinafter referred to as the unmanned vehicle) (the current position point and the target unmanned vehicle motion state corresponding to the current position point) as the starting state, and searches for a short-term optimal decision trajectory (hereinafter referred to as the optimal trajectory); the i-th short-term planning cycle takes the end state of the optimal decision trajectory of the i-1-th short-term planning cycle (that is, the end point and the target unmanned vehicle motion state corresponding to the end point) as the starting state for short-term planning. Assume that the time complexity of each short-term plan is o(N d ), then the complexity of the entire long-term planning is o(N d N i ).

[0039] The principle diagram of the i-th short-term planning cycle is as follows Figure 3 As shown. The i-th short-term planning cycle can be divided into K i The first sampling stage starts from the initial state of the short-term plan and uniformly samples the target reachable set (referred to as the reachable set); assuming that the number of discrete target points (referred to as target points) in the reachable set is Connect the starting point and the target point in the reachable set respectively, and we can get The second sampling stage uses all target points in the target reachable set of the first sampling stage (all specific target points are used in the embodiment) as starting points, and uniformly samples the target reachable set. Assume that the discrete target point corresponding to each starting point is candidate trajectories. And so on, when we reach the Kth i During the sampling phase, we can get From these candidate trajectories, a candidate trajectory with the minimum cost is selected as the optimal decision trajectory for the short-term planning cycle.

[0040] To facilitate understanding, the trajectory search method proposed in this application is further briefly described below:

[0041] First, according to the current motion state of the target unmanned vehicle, the number of sampling stages and the duration of each sampling stage in the first short-term plan are determined. Then, in the first short-term plan, based on the current position point and current motion state of the target unmanned vehicle, the target reachable set (sampling point set) of the first sampling stage is determined, and the candidate trajectories and their costs in the first sampling stage are further calculated. Then, based on the specific target points in the target reachable set of the first sampling stage, the target reachable set and the candidate trajectories and their costs of the second sampling stage are determined. This cycle is repeated until the target reachable set and the candidate trajectories and their costs of the last sampling stage in the first short-term plan are obtained. Then, based on the candidate trajectories and their costs in the last sampling stage in the first short-term plan, the optimal decision trajectory of the first short-term plan is determined. Then, based on the end point of the optimal decision trajectory of the first short-term plan and the corresponding motion state of the target unmanned vehicle, the optimal decision trajectory of the next short-term plan is determined until the optimal decision trajectory of the last short-term plan is obtained. Finally, the final decision trajectory is determined based on the optimal decision trajectories of all short-term plans.

[0042] As an optional implementation, step 101 specifically includes:

[0043] Step 101.1, determine the duration of the current short-term plan based on the current movement speed and maximum braking acceleration of the target unmanned vehicle corresponding to the starting point of the current short-term plan.

[0044] Step 101.2: Determine the number of sampling phases and the duration of the sampling phases in the current short-term plan according to the duration of the current short-term plan.

[0045] For the number of sampling stages K in short-term planning i and the duration of the sampling phase (time window) t d , further explanation is given below.

[0046] The number of sampling stages refers to the number of sampling stages within a short-term plan, and the duration of the sampling stage or time window refers to the time of a single sampling stage within a short-term plan.

[0047] Assume that the starting point of sampling in a certain sampling stage is Among them, s0, and They represent the longitudinal displacement, longitudinal velocity and longitudinal acceleration of the vehicle in the Frenet coordinate system, l0, and They represent the lateral displacement, lateral velocity and lateral acceleration of the vehicle in the Frenet coordinate system. Since the trajectory planning task only considers the vehicle's forward direction,

[0048] In some embodiments, in order to ensure that a single short-term plan allows for a decision-making behavior of stopping maneuvers, the time domain (duration) of a single short-term plan is t min The calculation is as follows:

[0049]

[0050] Where a min <0 is the maximum braking acceleration of the unmanned vehicle, Indicates rounding up.

[0051] The number of sampling stages K for a single short-term plan i The calculation is as follows:

[0052]

[0053] Where, t d,min Indicates the minimum time of a single sampling phase that is preset. Indicates rounding down.

[0054] Then the sampling time t of a single sampling stage is d The calculation is as follows:

[0055]

[0056] K i The physical meaning represented is: in the short-term planning, K i The changes in driving behavior (such as acceleration, deceleration and lane change) ensure the flexibility of driving decisions within the short-term planning. i When =2, the balance between planning efficiency and flexibility is optimal.

[0057] As an optional implementation, step 102 specifically includes:

[0058] Step 102.11, when the current sampling stage is the first sampling stage in the current short-term plan, determine the longitudinal sampling range of the first sampling stage based on the duration of the sampling stage, the current position of the target unmanned vehicle, and the motion state of the target unmanned vehicle corresponding to the current position.

[0059] Step 102.12: Determine the lateral sampling range of the first sampling phase based on the lane where the current position point is located and the adjacent lanes.

[0060] Step 102.13: sampling target points within the road area defined by the longitudinal sampling range and the transverse sampling range to obtain a reachable target set for the first sampling phase.

[0061] Furthermore, step 102 specifically includes:

[0062] Step 102.21, when the current sampling stage is the mth sampling stage in the current short-term plan, determine the longitudinal sampling range of the mth sampling stage corresponding to each specific target point based on the duration of the sampling stage, each specific target point in the target reachable set of the m-1th sampling stage, and the motion state of the target unmanned vehicle corresponding to each specific target point; where 2≤m≤the number of sampling stages.

[0063] Step 102.22: Determine the lateral sampling range of the mth sampling phase corresponding to each specific target point based on the lane where each specific target point is located and the adjacent lanes.

[0064] Step 102.23, target point sampling is performed within the road area defined by the longitudinal sampling range and the transverse sampling range of the mth sampling stage corresponding to each specific target point, to obtain the target reachable subset of the mth sampling stage corresponding to each specific target point.

[0065] Step 102.24: Determine the target reachable set of the mth sampling stage according to the reachable subsets of the mth sampling stage corresponding to all specific target points in the target reachable set of the m-1th sampling stage.

[0066] The calculation of the target reachable set for a single sampling stage in short-term planning is further explained as follows.

[0067] It is still assumed that the sampling starting point of a certain sampling stage is The relative vertical sampling range of the target reachable subset corresponding to the sampling starting point is expressed as [s min ,s max ], where s min is the minimum longitudinal distance, calculated as follows:

[0068]

[0069] s max is the maximum longitudinal distance, calculated as follows:

[0070]

[0071] Where a max >0 is the maximum forward acceleration (longitudinal acceleration) of the target unmanned vehicle, v max is the target maximum speed of the unmanned vehicle.

[0072] In addition, to help understand the calculation process of the lateral sampling range, it is assumed that the lateral coordinate set of each lane centerline in the Frenet coordinate system is expressed as m numIndicates the total number of lanes, and m indicates the mth lane starting from the rightmost lane. In order to conveniently determine the lane number of the unmanned vehicle, we also need to obtain the horizontal coordinate of the boundary line of each lane in the Frenet coordinate system, which is represented by the set l boundary :

[0073]

[0074] The horizontal coordinates of the two boundary lines corresponding to the mth lane in the Frenet coordinate system are in

[0075] Take the 3.5m wide 3 lanes in the scene as an example, center ={-3.5,0,3.5} means the horizontal coordinate values of the center lines of each lane in the Frenet coordinate system are -3.5m, 0m, and 3.5m respectively. At this time, the boundary lines of each lane are l boundary ={-5.25,-1.75,1.75,5.25}.

[0076] Therefore, the lateral offset l0 corresponding to the sampling starting point in this sampling phase should satisfy This constraint states that the starting point is within the lane. To determine whether l0 is in the mth lane, if the inequality holds, it means that the starting point is in the mth lane. Here we assume that l0 is in the mth lane.

[0077] The horizontal sampling range of the target reachable subset corresponding to the sampling starting point is expressed as [l min ,l max ], where l min is the minimum lateral distance in this sampling phase, calculated as follows:

[0078]

[0079] m low =max(m-1,1);

[0080] l max is the maximum lateral distance in the current sampling phase, calculated as follows:

[0081]

[0082] m up =min(m+1,m num );

[0083] Let the target reachable subset corresponding to the sampling starting point be where N i N is the number of samples in the vertical direction of the target reachable subset, which can be specified by the user; jN is the number of samples of the target reachable subset in the horizontal direction. j =N lane (m up -m low ), N lane Indicates the number of samples in the transverse direction of each lane, which can be specified by the user. i ,l j The specific value of is calculated as follows:

[0084]

[0085] As an optional implementation, each candidate trajectory in each sampling phase is calculated using a time-normalized fifth-order polynomial.

[0086] Connect the starting points using a time-normalized quintic polynomial and end point The normalized trajectory is defined as follows:

[0087]

[0088] Among them, s(t) and l(t) are the vertical and horizontal parts of the normalized trajectory, respectively.

[0089] In the vertical direction, the normalized trajectory needs to satisfy the following constraints:

[0090]

[0091] The calculation formula for the 6 unknown polynomial coefficients: a0, a1, a2, a3, a4 and a5 is as follows:

[0092]

[0093] The inverse matrix of E can be calculated offline, improving solution efficiency. The offline-calculated matrix E can be used when calculating the horizontal portion of the normalized trajectory l(t) or candidate trajectories at other stages.

[0094] In the horizontal direction, the solution method for the polynomial coefficients b0, b1, b2, b3, b4 and b5 is the same.

[0095] Map the normalized trajectory calculated above back to time t∈[t0,t0+t d ], just need to The candidate trajectories obtained are as follows:

[0096]

[0097] Wherein, t0 represents the starting time of the candidate trajectory.

[0098] As an optional implementation, the cost of each candidate trajectory is determined using a candidate trajectory cost calculation model; the calculation model for the cost of the candidate trajectory is:

[0099] c=c0+c soomth +c ref +c goal +c safe +c SOTIF ;

[0100] Where c represents the cost of the current candidate trajectory; c0 represents the cost of the candidate trajectory in the previous sampling stage with the starting point of the current candidate trajectory as the end point. When the current candidate trajectory is the candidate trajectory in the first sampling stage, c0 is equal to 0; c soomth represents the smoothness cost of the current candidate trajectory; c ref represents the reference line cost of the current candidate trajectory; c goal represents the target speed cost of the current candidate trajectory; c safe represents the collision cost of the current candidate trajectory; c SOTIF Represents the expected functional safety index of the current candidate trajectory.

[0101] As an optional implementation, the smoothness cost c of the candidate trajectory is soomth The determination process is:

[0102] According to the smoothness cost calculation model, the smoothness cost c of the candidate trajectory is determined soomth ; The smoothness cost calculation model is:

[0103]

[0104] Among them, s cur (t) represents the longitudinal trajectory component of the candidate trajectory, s pre (t) represents the longitudinal trajectory component of the candidate trajectory at the previous sampling stage connected to the candidate trajectory; Indicates s cur The second-order differential of (t); Indicates s pre The second-order differential of (t); k soomth The weight coefficient representing the smoothness cost is given by the user; t0 represents the starting time of the candidate trajectory; t d Indicates the duration of the candidate trajectory, that is, the duration of the sampling phase.

[0105] As an optional implementation, the reference line cost c of the candidate trajectory ref The determination process is:

[0106] According to the reference line cost calculation model, the reference line cost c of the candidate trajectory is determinedref ; The reference line cost calculation model is:

[0107]

[0108] Among them, W lane Indicates the distance between adjacent lane lines, that is, lane width; l cur (t0+t d ) represents t0+t d The lateral trajectory component value of the candidate trajectory at time k ref Indicates the weight coefficient of the reference line cost.

[0109] As an optional implementation, the target speed cost c of the candidate trajectory goal The determination process is:

[0110] According to the target speed cost calculation model, the target speed cost c of the candidate trajectory is determined goal The target speed cost calculation model is:

[0111]

[0112] Among them, v des Indicates the expected speed of the target unmanned vehicle, provided by other modules; s cur (t) represents the longitudinal trajectory component of the candidate trajectory; Indicates s cur The first-order differential of (t); k goal The weight coefficient representing the target speed cost.

[0113] As an optional implementation, the collision cost c safe The determination process includes the following steps:

[0114] Step 301: Perform equal time interval sampling on the current candidate trajectory to obtain a number of sampling points.

[0115] Step 302: Determine whether the target unmanned vehicle will collide at all sampling points, and obtain a determination result.

[0116] Step 303: If the result of the judgment is yes, the collision cost c safe Set to a preset fixed value.

[0117] Step 304: If the result of the judgment is negative, the collision cost c safe Set to 0.

[0118] That is, for t∈[t0,t0+t d ] Perform equal-interval sampling, the nth sampling moment n=0,1,...,nto , n to The sampling number parameter is customized. If the current candidate trajectory is at any time t n If both of them collide, the current candidate trajectory is considered to have collided. safe It can be expressed as follows:

[0119]

[0120] In actual calculations, since computers cannot represent infinite numbers, a larger value (i.e., a preset fixed value) can be artificially set based on the actual situation to replace the infinity.

[0121] Because c safe It can only represent the candidate trajectories in the time domain [t0,t0+t d ] is safe, it is impossible to indicate whether the final state of the short-term plan allows a feasible safe stopping maneuver. Therefore, this application designs a c SOTIF , represents the expected functional safety index, that is, the safety index for evaluating the final state of short-term planning.

[0122] As an optional implementation, the expected functional safety index c SOTIF The determination process includes the following steps:

[0123] Step 201: When the current candidate trajectory is not the candidate trajectory of the last sampling stage in the current short-term plan, c SOTIF It is equal to 0, that is, only the expected functional safety of the candidate trajectory in the last sampling stage needs to be considered, and the expected functional safety of the candidate trajectory in the previous sampling stage does not need to be considered.

[0124] In step 202, when the current candidate trajectory is the candidate trajectory of the last sampling stage in the current short-term plan, the target reachable set of the safe stop maneuvering stage of the current short-term plan is determined based on the end point of the current candidate trajectory and the motion state of the target unmanned vehicle corresponding to the end point of the current candidate trajectory.

[0125] Step 203: Determine each candidate trajectory in the safe stop maneuvering phase based on each target point in the target reachable set for the safe stop maneuvering phase and the target unmanned vehicle motion state corresponding to each target point, and the end point of the current candidate trajectory and the target unmanned vehicle motion state corresponding to the end point of the current candidate trajectory; wherein the starting point of each candidate trajectory in the safe stop maneuvering phase is the end point of the current candidate trajectory; and the end point of each candidate trajectory in the safe stop maneuvering phase is the target point in the target reachable set for the safe stop maneuvering phase.

[0126] Step 204: Determine the safety index SOTIF(*) of each candidate trajectory in the safe stop maneuver phase according to a safety index calculation model; the safety index calculation model is:

[0127]

[0128] Among them, SOTIF (P z ) represents the candidate trajectory P in the safe stop maneuver phase z Safety index; Represents the candidate trajectory P z The first collision time t obs The corresponding forward acceleration of the target unmanned vehicle.

[0129] Step 205: Select the minimum safety index and determine it as c SOTIF value.

[0130] As an optional implementation, step 202 specifically includes:

[0131] Step 202.1: Determine the duration of the safe stop maneuver phase based on the motion state of the target unmanned vehicle corresponding to the end point of the current candidate trajectory.

[0132] The duration of the safety stop maneuver phase is no longer t d , but t min .

[0133] Step 202.2: Determine the longitudinal sampling range of the safety stop maneuver phase based on the duration of the safety stop maneuver phase, the end point of the current candidate trajectory, and the motion state of the target unmanned vehicle corresponding to the end point of the current candidate trajectory.

[0134] Step 202.3: Determine the lateral sampling range for the safe stop maneuver phase based on the lane where the end point of the current candidate trajectory is located and the adjacent lanes.

[0135] Step 202.4: sampling target points within the road area defined by the longitudinal sampling range and the transverse sampling range of the safe stop maneuvering phase to obtain a reachable target set for the safe stop maneuvering phase.

[0136] In summary, the c of each candidate trajectory in the last sampling stage can be SOTIF The calculation steps are summarized as follows:

[0137] 1) Using the end point of the candidate trajectory as the starting point, perform target reachable set calculation similar to step 105. However, the time window is not t d But t min , and the number of sampling stages is 1.

[0138] 2) Generate the candidate trajectory set corresponding to the target reachable set in the safe stop maneuver phase. The candidate trajectory is expressed as N z represents the number of candidate trajectories in the safe stop maneuver phase; s z (t) represents the candidate trajectory P z The longitudinal trajectory component of l z (t) represents the candidate trajectory P z The lateral trajectory component.

[0139] 3) Calculate the safety index SOTIF (P z ).

[0140] For t∈[t0,t0+t min ] Perform equal-interval sampling, the nth sampling moment n=0,1,...,n to , n to The number of samples is a parameter that can be set by user. n If both of them collide, the candidate trajectory is considered to have collided. Assume that the first collision time of the candidate trajectory is t obs , the candidate trajectory P z Safety index SOTIF (P z ) is calculated as follows:

[0141]

[0142] in, The first collision time is t obs The forward acceleration (longitudinal acceleration) value at .

[0143] 4) Calculate c SOTIF , calculated as follows:

[0144]

[0145] c SOTIF This is mainly to evaluate the trajectory of the last stage in a short-term plan. This makes short-term planning more forward-looking. For example, there is a scenario where an obstacle is not far from the final state of the last stage. In the next planning, as long as the obstacle is close enough to the final state, it may happen that even deceleration at maximum acceleration cannot guarantee safety. SOTIF The role of is to take the last stage state as the starting point and consider one step forward. When the above situation occurs, c SOTIF It will not be zero, if it exists, it is c SOTIF= 0, the subsequent solution process will give priority to candidate trajectories that will not collide in the future. Generally speaking, according to the calculations in the sampling phase and planning time domain mentioned above, there is a c SOTIF = 0. If c SOTIF If all are not 0, it usually means that an obstacle appears suddenly. In this case, the trajectory that causes collision at the minimum speed will be given priority.

[0146] The short-term planning process for this application can be summarized as follows:

[0147] First determine the number of stages K in the short-term plan i i and the planning time domain t for each sampling stage d , then the short-term planning process is as follows:

[0148] 1) In the first sampling stage, based on the sampling starting point (i.e., the starting point of the short-term plan i), a target reachable set is generated, a set of candidate trajectories from the sampling starting point to the target reachable set is generated, and the cost of the candidate trajectories in the candidate trajectory set is calculated.

[0149] 2) 2 ≤ k < K i In the sampling phase, the candidate trajectories of the k-1th sampling phase are traversed. If the cost value is ∞ (or greater than or equal to the preset cost threshold), it indicates that the candidate trajectories of the k-1th sampling phase collide, and no operation is performed. If the cost value is not ∞ (or less than the preset cost threshold), the end point of the candidate trajectory of the k-1th sampling phase is used as the sampling starting point to generate a target reachable set and a candidate trajectory set from the sampling starting point to the target reachable set. The cost of the candidate trajectories in the candidate trajectory set is calculated.

[0150] 3) kth = K i In the sampling phase (i.e., the last sampling phase), the candidate trajectories in the k-1th sampling phase are traversed. If the cost value is ∞ (or greater than or equal to the preset cost threshold), it indicates that the candidate trajectories in the k-1th sampling phase collide, and no operation is performed. If the cost value is not ∞ (or less than the preset cost threshold), the end point of the candidate trajectory in the k-1th sampling phase is used as the sampling starting point to generate a target reachable set and a candidate trajectory set from the sampling starting point to the target reachable set. The cost of the candidate trajectories in the candidate trajectory set is calculated.

[0151] 4) Sort the cost values of the trajectory selection in the last sampling stage, select the candidate trajectory corresponding to the smallest cost value, and trace back to the starting point of the short-term plan (that is, the starting point of the first sampling stage) through each sampling stage in turn to obtain a trajectory, which is the optimal decision trajectory of the short-term plan i.

[0152] Since the starting point of a candidate trajectory in each sampling stage except the first sampling stage is the end point of a candidate trajectory in the previous sampling stage, the optimal decision trajectory is determined when the candidate trajectory with the minimum cost value in the last sampling stage is selected.

[0153] The long-term planning process for this application can be summarized as follows:

[0154] Assume that long-term planning is done by N i The first short-term planning cycle starts with the current state of the target unmanned vehicle and searches for a short-term optimal decision trajectory. The i>1 short-term planning cycle starts with the end point (end point) of the optimal decision trajectory of the i-1 short-term planning cycle. Finally, the optimal decision trajectory of each short-term plan is connected to obtain the final decision trajectory.

[0155] This application belongs to the field of unmanned vehicle trajectory planning, and relates to the generation of decision trajectories, and in particular to how to embed complex constraints while taking into account search efficiency in the process of decision trajectory generation, and to ensure that there is always a safe and feasible solution (this application determines the planning time domain and number of stages with maximum acceleration to ensure that there must be a stop-maneuver behavior). This application is easier to embed more complex constraints than the common hybrid AStar algorithm and is suitable for structured road scenes. It is more flexible than the dynamic programming search method. Since a safe and feasible trajectory can always be planned (at least including a stop-maneuver decision behavior), it has more practical value than the two methods.

[0156] To ensure practicality, the method proposed in this application embeds a variety of complex constraints, including lane constraints, speed and acceleration constraints, lane change constraints, etc.

[0157] To improve efficiency, this application uses a combination of short-term traversal search and long-term greedy search. Short-term traversal search ensures the search flexibility of the trajectory, while long-term greedy search ensures the search efficiency of the trajectory.

[0158] In order to ensure the continued feasibility of the trajectory, the time window and the number of sampling stages can be adaptively adjusted according to the current state of the vehicle, so that the vehicle always has a trajectory that can stop. At the same time, this application designs a collision assessment function (see c SOTIF When a collision is unavoidable, you can choose to collide with the vehicle in a way that minimizes its momentum.

[0159] In an exemplary embodiment, a computer device is provided. The computer device may be a server or a terminal. The internal structure diagram thereof may be as follows: Figure 4As shown. The computer device includes a processor, a memory, an input / output interface (Input / Output, abbreviated as I / O) and a communication interface. The processor, memory and input / output interface are connected through a system bus, and the communication interface is connected to the system bus through the input / output interface. The processor of the computer device is used to provide computing and control capabilities. The memory of the computer device includes a non-volatile storage medium and an internal memory. The non-volatile storage medium stores an operating system, a computer program and a database. The internal memory provides an environment for the operation of the operating system and computer program in the non-volatile storage medium. The database of the computer device is used to store trajectory search related data. The input / output interface of the computer device is used to exchange information between the processor and an external device. The communication interface of the computer device is used to communicate with an external terminal through a network connection. When the computer program is executed by the processor, a hybrid search method for decision trajectories of an unmanned vehicle is implemented.

[0160] Those skilled in the art will understand that Figure 4 The structure shown in the figure is merely a block diagram of a portion of the structure related to the solution of the present application and does not constitute a limitation on the computer device to which the solution of the present application is applied. A specific computer device may include more or fewer components than shown in the figure, or combine certain components, or have a different component arrangement. In an exemplary embodiment, a computer device is provided, including a memory and a processor. The memory stores a computer program, and the processor implements the steps of the above-mentioned method embodiments when executing the computer program.

[0161] In an exemplary embodiment, a computer-readable storage medium is provided, storing a computer program. When the computer program is executed by a processor, the steps in the above-mentioned method embodiments are implemented.

[0162] In an exemplary embodiment, a computer program product is provided, including a computer program. When the computer program is executed by a processor, the steps in the above method embodiments are implemented.

[0163] The technical features of the above embodiments can be combined arbitrarily. To make the description concise, not all possible combinations of the technical features in the above embodiments are described. However, as long as there is no contradiction in the combination of these technical features, they should be considered to be within the scope of this specification.

[0164] This document uses specific examples to illustrate the principles and implementation methods of this application. The description of the above examples is only intended to help understand the method and core concept of this application. At the same time, for those skilled in the art, based on the concept of this application, there may be changes in the specific implementation methods and application scope. In summary, the content of this specification should not be understood as limiting this application.

Claims

1. A hybrid search method for unmanned vehicle decision trajectories, characterized in that: include: In the current short-term plan, the number and duration of sampling phases are determined based on the target unmanned vehicle's motion state corresponding to the starting point of the current short-term plan. The starting point of each short-term plan except the first short-term plan is the end point of the optimal decision trajectory of the previous short-term plan; Determine the reachable target set for the current sampling phase based on the duration of the sampling phase, the starting point of the current sampling phase, and the motion state of the target unmanned vehicle corresponding to the starting point. The starting points of each sampling phase other than the first sampling phase are the specific target points in the reachable target set for the previous sampling phase. The specific target points are the end points of the candidate trajectories in the previous sampling phase whose cost is less than a preset cost threshold. Determine each candidate trajectory and its cost in the current sampling phase based on the target points in the reachable target set of the current sampling phase and the target unmanned vehicle motion state corresponding to each target point, the starting point of the current sampling phase and the target unmanned vehicle motion state corresponding to the starting point of the current sampling phase; the end point of each candidate trajectory in the current sampling phase is the target point in the reachable target set of the current sampling phase; Repeat the above process until the determination of each candidate trajectory and the cost of each candidate trajectory in the last sampling stage of the current short-term plan is completed; Determine the optimal decision trajectory for the current short-term plan based on the candidate trajectories and their costs in the last sampling phase of the current short-term plan; Determine whether the current short-term plan is the last short-term plan. If it is, determine the final decision trajectory based on the optimal decision trajectory of all short-term plans.

2. The hybrid search method for unmanned vehicle decision trajectories according to claim 1 is characterized in that: The cost of each candidate trajectory is determined using a candidate trajectory cost calculation model; the cost calculation model of the candidate trajectory is: c=c0+c soomth +c ref +c goal +c safe +c SOTIF ; Where c represents the cost of the current candidate trajectory; c0 represents the cost of the candidate trajectory in the previous sampling stage with the starting point of the current candidate trajectory as the end point. When the current candidate trajectory is the candidate trajectory in the first sampling stage, c0 is equal to 0; c soomth represents the smoothness cost of the current candidate trajectory; c ref represents the reference line cost of the current candidate trajectory; c goal represents the target speed cost of the current candidate trajectory; c safe represents the collision cost of the current candidate trajectory; c SOTIF Represents the expected functional safety index of the current candidate trajectory.

3. The hybrid search method for unmanned vehicle decision trajectories according to claim 2, characterized in that: The expected functional safety index c SOTIF The determination process is: When the current candidate trajectory is not the candidate trajectory of the last sampling stage in the current short-term plan, c SOTIF =0; When the current candidate trajectory is the candidate trajectory of the last sampling stage in the current short-term plan, the target reachable set of the safe stop maneuvering stage of the current short-term plan is determined according to the end point of the current candidate trajectory and the motion state of the target unmanned vehicle corresponding to the end point of the current candidate trajectory; Determine each candidate trajectory in the safe stop maneuvering phase according to each target point in the target reachable set of the safe stop maneuvering phase and the target unmanned vehicle motion state corresponding to each target point, and the end point of the current candidate trajectory and the target unmanned vehicle motion state corresponding to the end point of the current candidate trajectory; wherein the starting point of each candidate trajectory in the safe stop maneuvering phase is the end point of the current candidate trajectory; and the end point of each candidate trajectory in the safe stop maneuvering phase is the target point in the target reachable set of the safe stop maneuvering phase; The safety index SOTIF(*) of each candidate trajectory in the safe stop maneuver phase is determined according to the safety index calculation model; the safety index calculation model is: Among them, SOTIF (P z ) represents the candidate trajectory P in the safe stop maneuver phase z Safety index; Represents the candidate trajectory P z The first collision time t obs The corresponding target unmanned vehicle forward acceleration; Select the minimum safety index and determine it as c SOTIF value.

4. The hybrid search method for unmanned vehicle decision trajectories according to claim 3 is characterized in that: According to the end point of the current candidate trajectory and the motion state of the target unmanned vehicle corresponding to the end point of the current candidate trajectory, the target reachable set of the current short-term planning safe stop maneuvering phase is determined, specifically including: Determine the duration of the safe stop maneuver phase based on the target unmanned vehicle's motion state corresponding to the end point of the current candidate trajectory; Determining a longitudinal sampling range for the safe stop maneuver phase based on the duration of the safe stop maneuver phase, the end point of the current candidate trajectory, and the motion state of the target unmanned vehicle corresponding to the end point of the current candidate trajectory; Determine the lateral sampling range during the safe stop maneuver phase based on the lane where the end point of the current candidate trajectory is located and the adjacent lanes. Target point sampling is performed within a road area defined by a longitudinal sampling range and a lateral sampling range during the safe stop maneuvering phase to obtain a target reachable set during the safe stop maneuvering phase.

5. The hybrid search method for unmanned vehicle decision trajectories according to claim 1, characterized in that: In the current short-term plan, the number and duration of sampling phases are determined based on the target unmanned vehicle's motion state corresponding to the starting point of the current short-term plan. Specifically, the number and duration of sampling phases in the current short-term plan are: Determine the duration of the current short-term plan based on the current speed and maximum braking acceleration of the target unmanned vehicle corresponding to the starting point of the current short-term plan; According to the duration of the current short-term plan, the number and duration of the sampling phases in the current short-term plan are determined.

6. The hybrid search method for unmanned vehicle decision trajectories according to claim 1, characterized in that: According to the duration of the sampling phase, the starting point of the current sampling phase and the motion state of the target unmanned vehicle corresponding to the starting point, the target reachable set of the current sampling phase is determined, specifically including: When the current sampling phase is the first sampling phase in the current short-term plan, the longitudinal sampling range of the first sampling phase is determined according to the duration of the sampling phase, the current position of the target unmanned vehicle, and the motion state of the target unmanned vehicle corresponding to the current position; Determine a lateral sampling range of the first sampling phase according to the lane where the current position point is located and the adjacent lanes of the lane; Target point sampling is performed within the road area defined by the longitudinal sampling range and the transverse sampling range to obtain a target reachable set in the first sampling stage.

7. The hybrid search method for unmanned vehicle decision trajectories according to claim 6, characterized in that: Based on the duration of the sampling phase, the starting point of the current sampling phase, and the motion state of the target unmanned vehicle corresponding to the starting point, the reachable set of the target in the current sampling phase is determined, which specifically includes: When the current sampling phase is the mth sampling phase in the current short-term plan, the longitudinal sampling range of the mth sampling phase corresponding to each specific target point is determined based on the duration of the sampling phase, each specific target point in the target reachable set of the m-1th sampling phase, and the motion state of the target unmanned vehicle corresponding to each specific target point; where 2≤m≤the number of sampling phases; Determining a lateral sampling range of the mth sampling phase corresponding to each specific target point according to the lane where each specific target point is located and the adjacent lanes of the lane; Performing target point sampling within a road area defined by a longitudinal sampling range and a transverse sampling range of the mth sampling phase corresponding to each specific target point to obtain a target reachable subset of the mth sampling phase corresponding to each specific target point; According to the m-th sampling stage reachable subsets corresponding to all specific target points in the m-1-th sampling stage target reachable set, the m-th sampling stage target reachable set is determined.

8. A computer device comprising: A memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor executes the computer program to implement the hybrid search method for unmanned vehicle decision trajectories according to any one of claims 1 to 7.

9. A computer-readable storage medium having a computer program stored thereon, characterized in that: When the computer program is executed by a processor, the unmanned vehicle decision trajectory hybrid search method according to any one of claims 1 to 7 is implemented.

10. A computer program product comprising a computer program, characterized in that When the computer program is executed by a processor, the unmanned vehicle decision trajectory hybrid search method according to any one of claims 1 to 7 is implemented.

Images