A method, device and equipment for trajectory planning of a curb parking, and a storage medium

Abstract

The application discloses a kind of parking trajectory planning method, according to the position of self-vehicle, target lane and end position, the predicted trajectory of obstacle is projected, and the obstacle affecting lane change is obtained;In each preset time, according to the speed performance of self-vehicle and the position of obstacle, one or more lane change points are obtained;According to the lane change parameter, the corresponding target parking position is calculated for the lane change point, including one or more of end position, the position of static obstacle, brake comfort and no-parking area;In frenet coordinate system, the constraint of obstacle and road edge is established along s direction, and the continuity of trajectory, lateral deviation and its derivative constraint are considered, and the smooth lane change trajectory is calculated using quadratic programming solver;According to the minimum value of cost function, the lane change trajectory corresponding to the minimum value is confirmed as the target parking trajectory. In this way, smooth, safe, comfortable and executable trajectory can be generated in dynamic environment, and the effect of finally completing parking is guaranteed.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of path planning, in particular to a method and device for trajectory planning for parking by the roadside, equipment and a storage medium. BACKGROUND

[0002] For unmanned taxis, unmanned buses and other passenger-carrying tools, it is necessary to accurately stop at the passenger-specified pick-up / drop-off point / bus stop in various complex dynamic environments. In the scene of dense traffic flow, the vehicle needs to cross one lane or even multiple lanes to park by the roadside. Meanwhile, some parking points may be outside the road boundary, that is, non-regular structured roads, and the vehicle needs to be parked close enough to the roadside to facilitate passenger boarding and not to interfere with the normal driving vehicles on the road.

[0003] However, in a dynamic congestion scenario, the existing sampling-based method samples feasible path points in space, and the search-based method searches grid nodes to obtain feasible node connection methods, neither of which can cope with real-time changes in the environment.

[0004] Therefore, how to give a safe, comfortable and feasible lane-changing trajectory in a semi-structured scenario to facilitate passenger boarding and give a multi-dimensional unified evaluation method for multiple lane-changing trajectories in combination with the parking point to select the optimal parking route has become a problem to be solved. SUMMARY

[0005] Therefore, the embodiments of the present application provide a method and device for trajectory planning for parking by the roadside, equipment and a storage medium to ensure that smooth, safe, comfortable and executable trajectories can be generated in a dynamic environment in near real time, which is suitable for structured or semi-structured roads in various urban and non-urban areas, and can ensure the effect of finally completing parking by the roadside.

[0006] To achieve the above-mentioned purpose, the present application provides a method for trajectory planning for parking by the roadside, which comprises the following steps:

[0007] S1: Projecting the predicted trajectory of a static obstacle and / or a dynamic obstacle according to the position of the ego vehicle, the target lane and the end position to obtain an obstacle affecting lane changing;

[0008] S2: Obtaining one or more lane-changing points according to the speed performance of the ego vehicle and the position of the obstacle at each preset time;

[0009] S3: Calculating the corresponding target parking position for the lane-changing point according to the lane-changing parameter, wherein the lane-changing parameter comprises one or more of the end position, the position of the static obstacle, the brake comfort and the no-parking area;

[0010] S4: constraints of obstacles and road edges are established along the s direction in frenet coordinate system, and trajectory continuity and lateral offset and its derivative constraints are considered, and a quadratic programming solver is used to calculate a smooth lane-changing trajectory;

[0011] S5: a cost value is calculated for each lane-changing trajectory according to a cost function, and a lane-changing trajectory corresponding to the minimum cost value is confirmed as the target pull-over trajectory.

[0012] Optionally, the step S1 comprises:

[0013] B1: a plurality of preset time points are divided within a preset time, and the position of an obstacle affecting lane-changing is obtained according to the projection of a static obstacle and / or a predicted trajectory of a dynamic obstacle and the position of the ego vehicle and the target lane at each preset time point.

[0014] Optionally, the step B1 comprises:

[0015] a region of interest is confirmed; wherein the region of interest is a region from the right side of the ego vehicle to the two side edges of the target lane where the ego vehicle is completely located;

[0016] the position of the obstacle on the target lane is confirmed according to the region of interest;

[0017] wherein the left edge of the obstacle is located in the region of interest, and the left boundary of the additional margin of the obstacle is also located in the region of interest.

[0018] Optionally, the step S2 comprises:

[0019] one or more lane-changing points are obtained according to s, t and v1 at each preset time point;

[0020] wherein t represents each preset time point, s represents the position of the ego vehicle on the target lane in frenet coordinate system, and v1 represents the speed of the ego vehicle; the lane-changing point comprises one or more of an acceleration point, a deceleration point, a front vehicle yielding point, a rear vehicle overtaking point and a window middle point;

[0021] when the target lane has a static obstacle, a lane-changing point that bypasses the static obstacle is generated;

[0022] when the ego vehicle approaches a fork, a lane-changing point that successfully changes lanes is generated.

[0023] Optionally, the step S3 comprises:

[0024] if the lane-changing parameter comprises an end point position, the target parking position is located at a position that can be reached by the ego vehicle at the current speed;

[0025] If the lane-changing parameter comprises a position of a static obstacle, the target parking position is at a safe distance from the position of the static obstacle;

[0026] If the lane-changing parameter comprises a brake comfort, the target parking position is at a position that meets the brake comfort;

[0027] If the lane-changing parameter comprises a no-parking area, the target parking position is not in the no-parking area.

[0028] Optionally, the step S4 comprises:

[0029] Sampling along the s direction with an interval of Δs, the feasible region of each lane-changing point is calculated;

[0030] Determining the lateral offset and its derivative according to the speed of the ego vehicle;

[0031] The continuity of the trajectory can be calculated by the following formula:

[0032] l″ i+1 = l″ i + l″′ i Δs

[0033]

[0034]

[0035] In the formula, i represents the index of the trajectory point, l ref represents the first-order continuous reference line of the lateral offset, l represents the lateral offset of the trajectory, l' represents the first-order derivative of the lateral offset, l" represents the second-order derivative of the lateral offset, and l'" represents the third-order derivative of the lateral offset. Δs represents the longitudinal displacement of the trajectory, Δs 2 represents the square of the longitudinal displacement of the trajectory, and Δs 3 represents the cube of the longitudinal displacement of the trajectory.

[0036] The optimization objective function used in the quadratic programming solver is:

[0037]

[0038] In the formula, w1, w2, w3, and w4 represent the weight values of each term of the objective function, and J represents the error value of the trajectory. The smaller the error value, the better the smoothness of the trajectory.

[0039] Optionally, the cost function is:

[0040] C t = W yaw * D yaw + W lat * D lat + Wlon D lon W acl D acl W dtfsst D dtfsst

[0041] wherein C t represents the value of the trajectory, the smaller the value, the better the trajectory offside effect; W yaw represents the weight of the angle offset, D yaw represents the angle offset; W lat represents the weight of the lateral offset, D lat represents the lateral offset; W lon represents the weight of the longitudinal offset, D lon represents the longitudinal offset; W acl represents the weight of the offset from the center line, D acl represents the offset of the trajectory from the center line; W acl represents the weight of the offset from the center line, D acl represents the offset of the trajectory from the center line; W dtfsst represents the weight of the distance between the trajectory end point and the front static obstacle, D acl represents the distance between the trajectory end point and the front static obstacle.

[0042] In addition, in order to achieve the above-mentioned purpose, the application further provides a trajectory planning device for parking along the curb, which comprises:

[0043] A first detection unit is used for projecting the predicted trajectory of the static obstacle and / or the dynamic obstacle according to the position of the ego vehicle, the target lane and the end position, so as to obtain the obstacle affecting the lane change;

[0044] A second detection unit is used for obtaining one or more lane change points according to the position of the obstacle and the speed performance of the ego vehicle at each preset time;

[0045] An acquisition unit is used for calculating the corresponding target parking position for the lane change point according to the lane change parameter, wherein the lane change parameter comprises one or more of the end position, the position of the static obstacle, the brake comfort and the no-parking area;

[0046] A first generation unit is used for establishing the constraint of the obstacle and the road edge along the s direction in the frenet coordinate system, considering the trajectory continuity and the lateral offset and its derivative constraint, and using a quadratic programming solver to calculate a smooth lane change trajectory;

[0047] The second generation unit is used for calculating the corresponding cost value of the multiple lane-changing trajectories according to the cost function, and confirming the lane-changing trajectory corresponding to the minimum cost value as the target curb parking trajectory. In addition, in order to achieve the above object, the application further provides a curb parking trajectory planning device, which comprises a memory, a processor and a curb parking trajectory planning program stored in the memory and capable of running on the processor, and the curb parking trajectory planning program is configured to implement the steps of the curb parking trajectory planning method as described above.

[0048] In addition, in order to achieve the above object, the application further provides a computer readable storage medium, which stores a computer program, and the computer program is executed by a processor to implement the steps of the curb parking trajectory planning method as described above.

[0049] The technical scheme provided by the application projects the predicted trajectory of the static obstacle and / or the dynamic obstacle according to the position of the ego vehicle, the target lane and the end position, obtains the obstacle affecting the lane changing, obtains one or more lane-changing points according to the speed performance of the ego vehicle and the position of the obstacle at each preset time, calculates the corresponding target parking position for the lane-changing point according to the lane-changing parameter, wherein the lane-changing parameter comprises one or more of the end position, the position of the static obstacle, the brake comfort and the no-parking area, establishes the constraint of the obstacle and the road edge along the s direction in the frenet coordinate system, considers the trajectory continuity and the lateral offset and the derivative constraint thereof, and calculates the smooth lane-changing trajectory by using a quadratic programming solver, and calculates the corresponding cost value of the multiple lane-changing trajectories according to the cost function, and confirms the lane-changing trajectory corresponding to the minimum cost value as the target curb parking trajectory. Through the embodiment of the application, the smooth, safe, comfortable and executable trajectory can be generated in the dynamic environment in near real time, which is suitable for the structured or semi-structured road in various urban and non-urban areas, and the effect of finally completing the curb parking can be guaranteed. BRIEF DESCRIPTION OF DRAWINGS

[0050] Figure 1 A flowchart of a curb parking trajectory planning method provided by the application.

[0051] Figure 2 An example diagram of a lane-changing point provided by the application.

[0052] Figure 3 An example diagram of a lane-changing trajectory selection provided by the application.

[0053] Figure 4 A structure block diagram of a curb parking trajectory planning device embodiment of the application.

[0054] Figure 5It is a device structure schematic diagram of a hardware running environment related to an embodiment scheme of the present application.

[0055] The implementation, functional features and advantages of the present application will be further described with reference to the accompanying drawings in conjunction with embodiments. DETAILED DESCRIPTION

[0056] In order to make the technical problems to be solved by the present application, technical solutions and beneficial effects more clear and explicit, the present application will be further described in detail below in conjunction with the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are only for the purpose of explaining the present application and do not limit the present application.

[0057] In the subsequent description, the suffix such as "module", "component" or "unit" used for representing elements is only for the convenience of the description of the present application, and has no specific meaning. Therefore, "module", "component" or "unit" can be used mixedly.

[0058] It should be noted that the terms "first", "second" and the like in the specification and claims of the present application and the above-described drawings are used to distinguish similar objects, and do not necessarily describe a specific order or sequence.

[0059] Please refer to Figure 1 As shown in the figure, the present application provides a side parking trajectory planning method, which comprises:

[0060] Step S1: According to the position of the ego vehicle, the target lane and the end position, the projection of the predicted trajectory of the static obstacle and / or the dynamic obstacle is carried out, and the obstacle affecting the lane change is obtained.

[0061] Step S2: At each preset time, one or more lane change points are obtained according to the speed performance of the ego vehicle and the position of the obstacle.

[0062] Step S3: According to the lane change parameters, the corresponding target parking position is calculated for the lane change point, wherein the lane change parameters include one or more of the end position, the position of the static obstacle, the brake comfort and the no-parking area.

[0063] Step S4: In the frenet coordinate system, the constraints of the obstacle and the road edge along the s direction are established, and the trajectory continuity and the lateral offset and their derivative constraints are considered, and the smooth lane change trajectory is calculated by using a quadratic programming solver.

[0064] Step S5: According to the cost function, the corresponding cost value is calculated for the plurality of lane change trajectories, and the lane change trajectory corresponding to the minimum cost value is confirmed as the target side parking trajectory.

[0065] The side parking trajectory planning method provided by the application projects the predicted trajectory of the static obstacle and / or the dynamic obstacle according to the position of the ego vehicle, the target lane and the end position, obtains the obstacle affecting the lane change, obtains one or more lane change points according to the speed performance of the ego vehicle and the position of the obstacle in each preset time, calculates the corresponding target parking position for the lane change point according to the lane change parameter, wherein the lane change parameter includes one or more of the end position, the position of the static obstacle, the brake comfort and the no-parking area, establishes the constraint of the obstacle and the road edge along the s direction under the frenet coordinate system, considers the trajectory continuity and the lateral offset and the derivative constraint thereof, and calculates the smooth lane change trajectory by using a quadratic programming solver, and calculates the corresponding cost value for the plurality of lane change trajectories according to the cost function, and confirms the lane change trajectory corresponding to the minimum cost value as the target side parking trajectory. In this way, by the embodiment of the application, the smooth, safe, comfortable and executable trajectory can be generated in the dynamic environment in near real time, which is suitable for the structured or semi-structured road in various urban and non-urban areas, and the effect of completing the side parking can be guaranteed.

[0066] The trajectory planning refers to calculating a smooth trajectory by the initial state (including the position, speed and acceleration of the ego vehicle), the target state (including the parking position, speed and acceleration), the position of the obstacle and the constraint conditions such as dynamics and comfort, so that the vehicle can reach the target state along the trajectory. The trajectory planning usually includes two parts of path planning and speed planning: the path planning is responsible for calculating the smooth path from the position of the ego vehicle to the parking position.

[0067] The side parking trajectory planning method provided by the application can give one or more safe, comfortable and feasible lane change points in accordance with the kinematics principle according to the position and the future predicted trajectory of the dynamic or static obstacle between the current lane, the target lane and the parking position, in combination with the current position of the ego vehicle, and according to the time, speed, acceleration, lateral and longitudinal position under the frenet coordinate system. The target parking position near the parking position is given for each lane change point based on the trajectory information of the obstacle, and finally the smooth lane change trajectory is calculated by using a quadratic programming solver, and the target side parking trajectory is obtained.

[0068] As a specific embodiment, the step S1 specifically includes:

[0069] The plurality of preset time points are divided in the preset time, the position of the obstacle affecting the lane change is obtained according to the projection of the predicted trajectory of the static obstacle and / or the dynamic obstacle and the position of the ego vehicle and the target lane in each preset time.

[0070] In this embodiment, the preset time T is divided into multiple preset moments, such as t0 to tn. The preset time can be set according to actual conditions. For example, if the planning system only plans a trajectory of 0 to 8 seconds, the preset time T is 8 seconds; t can represent a time interval of 0 to 8 seconds, that is, it can be divided into 8 preset moments, such as t0 to t8.

[0071] In this embodiment, since obstacles affecting lane changes include obstacles in front of the ego vehicle, it is necessary to consider maintaining a safe distance between the ego vehicle and the obstacles in front. Furthermore, obstacles entering the target lane that may affect lane changes also need to be considered, and maintaining a safe distance between the ego vehicle and obstacles in the target lane is also necessary. Specifically, taking the example of the ego vehicle moving from the current lane to the target lane on the right, the method for obtaining the position of obstacles affecting lane changes at each preset time may include:

[0072] The region of interest (ROI) is determined, where the region of interest is the area from the right side of the vehicle to the two side edges of the target lane where the vehicle is completely within the target lane. The location of the obstacle in the target lane is then determined based on the ROI, where the left edge of the obstacle is within the ROI, and the left edge of the obstacle's additional margin is also within the ROI.

[0073] The additional margin distance of the obstruction is calculated using the following formula:

[0074] d extra =kv

[0075] In the formula, k represents the first coefficient, v represents the obstacle speed, d extra Indicates additional margin for obstacles.

[0076] The additional margin of the left edge of the obstruction is calculated using the following formula:

[0077]

[0078] Where, Indicates the left edge of the additional margin of the obstacle, l lef Indicates the left edge of the obstruction.

[0079] For example, Figure 2 As shown in the figure, the ego vehicle is represented by ego and is moving from the current lane to the target lane on the right. The obstacles are obs1, obs2, obs3, obs4, obs5, and obs6. The region of interest is the area between the current lane and the target lane. The obstacles that need to be considered when entering the target lane are obs3, obs4, obs5, and obs6. The obstacles that need to be kept at a safe distance at all times are obs1 and obs2.

[0080] As a specific embodiment, the step S2 specifically comprises:

[0081] At each preset time, one or more lane-changing points are obtained according to the vehicle speed performance and the position of the obstacle.

[0082] In this embodiment, one or more lane-changing points can be obtained according to s, t and v1 at each preset time; the lane-changing points need to meet the requirements of safety and vehicle performance comfort arrival.

[0083] In this embodiment, t is confirmed, s represents the position of the ego vehicle on the target lane in the frenet coordinate system, and v1 represents the speed of the ego vehicle. That is, in the case of considering the dynamic obstacle position, s can be at which place on the target lane, s here is the position of the ego vehicle, but is only the predicted position of the ego vehicle, not the current actual position, by sampling different s positions on the road, whether the vehicle can reach the s point at the time t with the speed v1 is searched.

[0084] Specifically, the lane-changing point can include one or more of an acceleration point, a deceleration point, a let-the-front-vehicle-go point, a pass-the-rear-vehicle point and a window-middle point. Of course, safety requirements, comfort requirements, traffic regulation requirements and additional requirements can also be considered.

[0085] Safety requirement: in the case of following the front vehicle, the distance between the vehicles needs to be greater than the distance traveled by the rear vehicle reaction time + the deceleration distance.

[0086] Comfort requirement: meet the vehicle dynamics constraints and comfort constraints.

[0087] Traffic regulation requirement: solid line and stop line.

[0088] Additional requirement: if the ego vehicle is already on the target lane, then the current lane-changing point is a suitable lane-changing point.

[0089] In this embodiment, when there is a static obstacle of long-term road congestion on the target lane, a lane-changing point that bypasses the static obstacle can be generated.

[0090] In this embodiment, when the ego vehicle approaches a fork and must change lanes, a lane-changing point that can successfully change lanes is generated.

[0091] Specifically, since multiple lane change points can be generated, a confidence cost can be given to each lane change point. When the distance to a lane change point is less than a first threshold, each lane change point can be evaluated. If the confidence cost of a lane change point is lower than a second threshold, the lane change point is considered available; if it is greater than a third threshold, the lane change point is directly deleted; if it is greater than or equal to the second threshold and less than or equal to the third threshold, a brake parameter is generated. If the brake parameter is less than a fourth threshold, the lane change point is retained, otherwise it is directly deleted. Finally, if no lane change point is available, a single-path lane change point is generated. If there is no access to a fork, but no lane change point is generated, a backup lane change point is generated.

[0092] As a specific embodiment, the step S3 specifically comprises:

[0093] According to the lane change parameters, a corresponding target parking position is calculated for the lane change point, wherein the lane change parameters include one or more of a terminal position, a position of a static obstacle, a brake comfort level, and a no-parking area.

[0094] In this embodiment, a reasonable target parking position is calculated for each lane change point, which needs to consider:

[0095] 1. Whether the lane change point distance from the terminal position, combined with the current speed, meets the kinematic reach.

[0096] 2. The lane change point distance from the position of the static obstacle needs to maintain a certain distance from the static obstacle.

[0097] 3. Whether the distance from the lane change point to the target parking position can be comfortably braked.

[0098] 4. The target parking position cannot be selected within the no-parking area.

[0099] That is, if the lane change parameters include the terminal position, the target parking position is located at a position that the ego vehicle can reach at the current speed; if the lane change parameters include the position of the static obstacle, the target parking position is at a safe distance from the position of the static obstacle; if the lane change parameters include the brake comfort level, the target parking position is at a position that meets the brake comfort level; if the lane change parameters include the no-parking area, the target parking position is not within the no-parking area.

[0100] Of course, the target parking position also needs to be close enough to the side of the road to facilitate passenger boarding and alighting.

[0101] In this embodiment, for each lane change point, a reasonable target parking position that is close to the side of the road, comfortable, and reasonable can be calculated according to the given terminal position, the position of the static obstacle, the brake comfort level, and the no-parking area.

[0102] As a specific embodiment, the step S4 specifically comprises:

[0103] In the frenet coordinate system, the constraints of the obstacles and the road edge are established along the s direction, and the trajectory continuity and the lateral offset and its derivative constraints are considered, and a quadratic programming solver is used to calculate the smooth lane-changing trajectory.

[0104] In this embodiment, the purpose is to generate a relatively smooth lane-changing trajectory. Since the frenet coordinate system is also called the road coordinate system, the coordinate axes are perpendicular to each other with the vehicle position as the origin, and are divided into the s direction (i.e. the normal direction of the current reference line, referred to as the longitudinal direction) and the l axis direction (i.e. the tangent direction along the road reference line, referred to as the lateral direction). Specifically, the feasible region of each lane-changing point can be calculated by sampling along the s direction at intervals of Δs, which is generally the width of the road center line on both sides. The lateral offset and its derivative can be calculated according to the vehicle speed, which will not be described here in this embodiment.

[0105] The continuity of the trajectory can be calculated by the following formula:

[0106] l″ i+1 =l″ i +l″′ i Δs

[0107]

[0108]

[0109] In the formula, i represents the index of the trajectory point, l ref represents the first-order continuous reference line of the lateral offset, l represents the lateral offset of the trajectory, l' represents the first-order derivative of the lateral offset, l" represents the second-order derivative of the lateral offset, and l'" represents the third-order derivative of the lateral offset. Δs represents the longitudinal displacement of the trajectory, Δs 2 represents the square of the longitudinal displacement of the trajectory, and Δs 3 represents the cube of the longitudinal displacement of the trajectory.

[0110] In this embodiment, the role of the quadratic programming solver is to input the optimization objective function and the constraints to calculate the optimal solution, which is the smooth lane-changing trajectory we want. That is, in the frenet coordinate system, the constraints of the obstacles and the road edge are established along the s direction, and the trajectory continuity and the lateral offset and its derivative constraints are considered, and the optimization objective function is input into the quadratic programming solver to calculate the smooth lane-changing trajectory.

[0111] The optimization objective function used in the quadratic programming solver is:

[0112]

[0113] In the formula, w1, w2, w3, w4 represent weight values of each item of the objective function, J represents an error value of the trajectory, and the smaller the error value, the better the smoothness of the trajectory.

[0114] As a specific embodiment, the step S5 specifically comprises:

[0115] According to the cost function, a corresponding cost value is calculated for each of the multiple lane-changing trajectories, and the lane-changing trajectory corresponding to the minimum cost value is determined as the target curb parking trajectory.

[0116] In this embodiment, according to the cost function and the information of the end point of the lane-changing trajectory and the target parking position, such as the angle (yaw), the lateral offset (lat), the longitudinal offset (lon), the cumulative offset from the center line (away_center_line), the distance to the front static obstacle (dist_to_front_static_st), etc., the optimal target curb parking trajectory is obtained.

[0117] The cost function is:

[0118] C t = W yaw *D yaw +W lat *D lat +W lon *D lon +W acl *D acl +W dtfsst *D dtfsst

[0119] In the formula, C t represents the cost value of the trajectory, and the smaller the cost value, the better the curb effect of the trajectory; W yaw represents the weight of the angle offset, D yaw represents the angle offset; W lat represents the weight of the lateral offset, D lat represents the lateral offset; W lon represents the weight of the longitudinal offset, D lon represents the longitudinal offset; W acl represents the weight of the offset of the trajectory from the center line, D acl represents the offset of the trajectory from the center line; W acl represents the weight of the offset from the center line, D acl represents the offset of the trajectory from the center line; W dtfsst represents the weight of the distance between the end point of the trajectory and the front static obstacle, D acl represents the distance between the end point of the trajectory and the front static obstacle.

[0120] It can be understood that the cost function term in the embodiment only considers static obstacles, because dynamic obstacles are by default moving and cannot evaluate the distance between the final parking position and the static obstacles. The static obstacles are to give the cost of being too close and needing to back up to get out of the car.

[0121] For example, as shown in Figure 3 The left-bound and right-bound correspond to the left and right boundaries of the road. The center-line represents the current road center line. The lat_dist represents the lateral distance between the parking position and the center of the target parking area, and the lon_ahead / behind_dist represents the longitudinal distance between the parking position and the target parking point. The rectangular area refers to the target parking area. The vehicle needs to be parked in the target parking area to meet the requirements. For the ego vehicle, the obstacle obs1 and the four lane change trajectories traj1, traj2, traj3 and traj4, since traj1 is too far from the endpoint in the lateral direction, traj2 is very close to the obstacle obs1, and traj4 is too far from the endpoint, in comparison, traj3 is the best.

[0122] In the embodiment of the application, one or more safe, comfortable and feasible lane change points that meet the kinematic principle can be given according to the current lane, the target lane and the position and future predicted trajectory of the dynamic or static obstacle between the parking position and the current position of the ego vehicle, and sampling is performed according to time, speed, acceleration, lateral and longitudinal positions in the frenet coordinate system. Based on the trajectory information of the obstacle, a target parking position near the parking position is given for each lane change point, and finally a smooth lane change trajectory is calculated by using a quadratic programming solver, and a target curb parking trajectory is obtained. In this way, a smooth, safe, comfortable and executable trajectory can be generated in a dynamic environment in near real time, which is suitable for structured or semi-structured roads in various urban and non-urban areas, and can ensure the effect of finally completing curb parking.

[0123] In addition, the embodiment of the application also provides a curb parking trajectory planning device, which refers to Figure 4 The curb parking trajectory planning device comprises:

[0124] The first detection unit 10 is used for projecting the predicted trajectory of the static obstacle and / or the dynamic obstacle according to the position of the ego vehicle, the target lane and the endpoint position, to obtain an obstacle that affects lane changing.

[0125] The second detection unit 20 is used for obtaining one or more lane change points according to the speed performance of the ego vehicle and the position of the obstacle at each preset time.

[0126] The acquisition unit 30 is configured to calculate a target parking position corresponding to the lane-changing point according to a lane-changing parameter, wherein the lane-changing parameter comprises one or more of a terminal position, a position of a static obstacle, a brake comfort level, and a no-parking area.

[0127] The first generation unit 40 is configured to establish constraints of the obstacle and the road edge along the s direction in the frenet coordinate system, and to calculate a smooth lane-changing trajectory by using a quadratic programming solver, considering the trajectory continuity and the lateral offset and its derivative constraints.

[0128] The second generation unit 50 is configured to calculate a cost value corresponding to a plurality of lane-changing trajectories according to a cost function, and to confirm the lane-changing trajectory corresponding to the minimum cost value as the target curb parking trajectory.

[0129] In the embodiment of the present application, one or more safe, comfortable, and feasible lane-changing points conforming to the kinematic principle can be given according to the positions of dynamic or static obstacles between the current lane, the target lane, and the parking position, the current position of the ego vehicle, and the sampling of time, speed, acceleration, and lateral and longitudinal positions in the frenet coordinate system. The target parking position near the parking position is given for each lane-changing point based on the trajectory information of the obstacle, and finally a smooth lane-changing trajectory is calculated by using a quadratic programming solver, so that the target curb parking trajectory is obtained. In this way, a smooth, safe, comfortable, and executable trajectory can be generated in a dynamic environment in near real time, which is suitable for structured or semi-structured roads in various urban and non-urban areas, and can ensure the effect of finally completing curb parking.

[0130] It should be noted that each unit in the above device can be used to implement each step in the above method, and the corresponding technical effects are achieved, and the present embodiment will not be described here.

[0131] Reference Figure 5 , Figure 5 The structural diagram of a device related to the hardware running environment of the embodiment of the present application.

[0132] As Figure 5As shown, the device can include a processor 1001, such as a CPU, a communication bus 1002, a user interface 1003, a network interface 1004, and a memory 1005. The communication bus 1002 is used to realize the connection communication between these components. The user interface 1003 can include a display screen (Display), an input unit such as a keyboard (Keyboard), and the optional user interface 1003 can also include a standard wired interface, a wireless interface. The network interface 1004 can optionally include a standard wired interface, a wireless interface (such as a WI-FI, 4G, 5G interface). The memory 1005 can be a high-speed RAM memory, or a stable memory (non-volatile memory) such as a magnetic disk memory. The memory 1005 can also be an independent storage device from the aforementioned processor 1001.

[0133] Those skilled in the art can understand that Figure 5 The structure shown in the foregoing embodiments does not constitute a limitation on the device, and can include more or fewer components than those shown, or combine certain components, or different component arrangements.

[0134] As Figure 5 As shown, the memory 1005 as a computer storage medium can include an operating system, a network communication module, a user interface module, and a curb parking trajectory planning program.

[0135] In Figure 5 In the device shown, the network interface 1004 is mainly used for data communication with an external network; the user interface 1003 is mainly used for receiving the input instructions of a user; the device calls the curb parking trajectory planning program stored in the memory 1005 through the processor 1001, and performs the following operations:

[0136] S1: According to the position of the ego vehicle, the target lane, and the end position, the predicted trajectory of the static obstacle and / or the dynamic obstacle is projected to obtain an obstacle affecting the lane change;

[0137] S2: At each preset time, one or more lane change points are obtained according to the speed performance of the ego vehicle and the position of the obstacle;

[0138] S3: According to the lane change parameters, the corresponding target parking position is calculated for the lane change point, wherein the lane change parameters include one or more of the end position, the position of the static obstacle, the brake comfort, and the no-parking area;

[0139] S4: In the frenet coordinate system, the constraints of the obstacle and the road edge are established along the s direction, and the trajectory continuity and the lateral offset and their derivative constraints are considered, and a quadratic programming solver is used to calculate a smooth lane change trajectory;

[0140] S5: According to the cost function, the corresponding cost value of the multiple lane-changing trajectories is calculated, and the lane-changing trajectory corresponding to the minimum cost value is confirmed as the target curb parking trajectory.

[0141] Optionally, the step S1 comprises:

[0142] B1: A plurality of preset time points are divided within a preset time, each preset time point is projected according to the predicted trajectory of the static obstacle and / or the dynamic obstacle, and the position of the obstacle affecting the lane-changing is obtained according to the position of the ego vehicle and the target lane.

[0143] Optionally, the step B1 comprises:

[0144] A region of interest is confirmed; wherein the region of interest is a region from the right side of the ego vehicle to the two side lines of the target lane where the ego vehicle is completely located;

[0145] The position of the obstacle on the target lane is confirmed according to the region of interest;

[0146] Wherein the left edge of the obstacle is located in the region of interest, and the left boundary of the additional margin of the obstacle is also located in the region of interest.

[0147] Optionally, the step S2 comprises:

[0148] At each preset time point, one or more lane-changing points are obtained according to s, t, v1;

[0149] Wherein t represents each preset time point, s represents the position of the ego vehicle on the target lane in the frenet coordinate system, and v1 represents the speed of the ego vehicle; the lane-changing point includes one or more of the acceleration point, the deceleration point, the front vehicle yielding point, the rear vehicle overtaking point and the window middle point;

[0150] When the target lane has a static obstacle, a lane-changing point that avoids the static obstacle is generated;

[0151] When the ego vehicle approaches a fork, a lane-changing point that successfully changes lanes is generated.

[0152] Optionally, the step S3 comprises:

[0153] If the lane-changing parameter includes the end point position, the target parking position is located at a position that the ego vehicle can reach at the current speed;

[0154] If the lane-changing parameter includes the position of the static obstacle, the target parking position is at a safe distance from the position of the static obstacle;

[0155] If the lane-changing parameter includes the brake comfort, the target parking position is at a position that meets the brake comfort;

[0156] If the lane-changing parameter includes a no-parking area, the target parking position is not in the no-parking area.

[0157] Optionally, the step S4 comprises:

[0158] Sampling along the s direction with interval Δs, the feasible region of each lane-changing point is calculated;

[0159] Determining the lateral offset and its derivative according to the speed of the ego vehicle;

[0160] The continuity of the trajectory can be calculated by the following formula:

[0161] l″ i+1 = l″ i + l″′ i Δs

[0162]

[0163]

[0164] wherein i represents the index of the trajectory point, l ref represents the 1st order continuous reference line of the lateral offset, l represents the lateral offset of the trajectory, l' represents the 1st order derivative of the lateral offset, l" represents the 2nd order derivative of the lateral offset, and l'" represents the 3rd order derivative of the lateral offset. Δs represents the longitudinal displacement of the trajectory, Δs 2 represents the square of the longitudinal displacement of the trajectory, and Δs 3 represents the 3rd power of the longitudinal displacement of the trajectory.

[0165] The optimization objective function used in the quadratic programming solver is:

[0166]

[0167] wherein w1, w2, w3, and w4 represent the weight values of each term of the objective function, and J represents the error value of the trajectory. The smaller the error value is, the better the smoothness of the trajectory is.

[0168] Optionally, the cost function is:

[0169] C t = W yaw *D yaw + W lat *D lat + W lon *D lon + W acl *D acl + W dtfsst *D dtfsst

[0170] wherein C trepresents the weight of the lateral offset, D yaw represents the weight of the angle offset, D yaw represents the angle offset; W lat represents the weight of the lateral offset, D lat represents the lateral offset; W lon represents the weight of the longitudinal offset, D lon represents the longitudinal offset; W acl represents the weight of the offset from the center line, D acl represents the offset of the trajectory from the center line; W acl represents the weight of the offset from the center line, D acl represents the offset of the trajectory from the center line; W dtfsst represents the weight of the distance between the trajectory end point and the front static obstacle, D acl represents the distance between the trajectory end point and the front static obstacle.

[0171] In the embodiments of the present application, one or more safe, comfortable and feasible lane change points can be given according to the positions of dynamic or static obstacles between the current lane, the target lane and the parking position, the future predicted trajectory, and the current position of the vehicle, and by sampling according to time, speed, acceleration, lateral and longitudinal positions in the frenet coordinate system. Based on the trajectory information of the obstacles, a target parking position near the parking position is given for each lane change point, and finally a smooth lane change trajectory is calculated by using a quadratic programming solver, so that a smooth, safe, comfortable and executable trajectory can be generated in a dynamic environment in near real time, and the effect of completing the side parking can be guaranteed, which is suitable for structured or semi-structured roads in various urban and non-urban areas.

[0172] In addition, the embodiments of the present application also provide a computer readable storage medium, and the computer readable storage medium stores a side parking trajectory planning program. When the side parking trajectory planning program is executed by a processor, the following operations are realized:

[0173] S1: Projecting the predicted trajectory of the static obstacle and / or the dynamic obstacle according to the vehicle position, the target lane and the end position to obtain an obstacle affecting the lane change;

[0174] S2: Obtaining one or more lane change points according to the vehicle speed performance and the position of the obstacle at each preset time;

[0175] S3: Calculating a corresponding target parking position for the lane change point according to the lane change parameters, wherein the lane change parameters include one or more of the end position, the position of the static obstacle, the brake comfort and the no-parking area;

[0176] S4: constraints of obstacles and road edges are established along the s direction in frenet coordinate system, and trajectory continuity and lateral offset and its derivative constraints are considered, and a quadratic programming solver is used to calculate a smooth lane-changing trajectory;

[0177] S5: a cost value is calculated for each lane-changing trajectory according to a cost function, and a lane-changing trajectory corresponding to the minimum cost value is confirmed as the target pull-over trajectory.

[0178] Optionally, the step S1 comprises:

[0179] B1: a plurality of preset time points are divided within a preset time, and the position of an obstacle affecting lane-changing is obtained according to the projection of a static obstacle and / or a predicted trajectory of a dynamic obstacle and the position of the ego vehicle and the target lane at each preset time point.

[0180] Optionally, the step B1 comprises:

[0181] a region of interest is confirmed; wherein the region of interest is a region from the right side of the ego vehicle to the two side edges of the target lane where the ego vehicle is completely located;

[0182] the position of the obstacle on the target lane is confirmed according to the region of interest;

[0183] wherein the left edge of the obstacle is located in the region of interest, and the left boundary of the additional margin of the obstacle is also located in the region of interest.

[0184] Optionally, the step S2 comprises:

[0185] one or more lane-changing points are obtained according to s, t and v1 at each preset time point;

[0186] wherein t represents each preset time point, s represents the position of the ego vehicle on the target lane in frenet coordinate system, and v1 represents the speed of the ego vehicle; the lane-changing point comprises one or more of an acceleration point, a deceleration point, a front vehicle yielding point, a rear vehicle overtaking point and a window middle point;

[0187] when the target lane has a static obstacle, a lane-changing point that bypasses the static obstacle is generated;

[0188] when the ego vehicle approaches a fork, a lane-changing point that successfully changes lanes is generated.

[0189] Optionally, the step S3 comprises:

[0190] if the lane-changing parameter comprises an end point position, the target parking position is located at a position that can be reached by the ego vehicle at the current speed;

[0191] If the lane-changing parameter comprises a position of a static obstacle, the target parking position is at a safe distance from the position of the static obstacle;

[0192] If the lane-changing parameter comprises a brake comfort, the target parking position is at a position that meets the brake comfort;

[0193] If the lane-changing parameter comprises a no-parking area, the target parking position is not in the no-parking area.

[0194] Optionally, the step S4 comprises:

[0195] Sampling along the s direction with an interval Δs, the feasible region of each lane-changing point is calculated;

[0196] Determining the lateral offset and its derivative according to the speed of the ego vehicle;

[0197] The continuity of the trajectory can be calculated by the following formula:

[0198] l″ i+1 = l″ i + l″′ i Δs

[0199]

[0200]

[0201] In the formula, i represents the index of the trajectory point, l ref represents the first-order continuous reference line of the lateral offset, l represents the lateral offset of the trajectory, l' represents the first-order derivative of the lateral offset, l" represents the second-order derivative of the lateral offset, and l'" represents the third-order derivative of the lateral offset. Δs represents the longitudinal displacement of the trajectory, Δs 2 represents the square of the longitudinal displacement of the trajectory, and Δs 3 represents the cube of the longitudinal displacement of the trajectory.

[0202] The optimization objective function used in the quadratic programming solver is:

[0203]

[0204] In the formula, w1, w2, w3, and w4 represent the weight values of each term of the objective function, and J represents the error value of the trajectory. The smaller the error value, the better the smoothness of the trajectory.

[0205] Optionally, the cost function is:

[0206] C t = W yaw * D yaw + W lat * D lat + Wlon *D lon +W acl *D acl +W dtfsst *D dtfsst

[0207] In the formula, C t represents the value of the trajectory, the smaller the value of the trajectory, the better the effect of the trajectory; W yaw represents the weight of the angle offset, D yaw represents the angle offset; W lat represents the weight of the lateral offset, D lat represents the lateral offset; W lon represents the weight of the longitudinal offset, D lon represents the longitudinal offset; W acl represents the weight of the offset from the center line, D acl represents the offset of the trajectory from the center line; W acl represents the weight of the offset from the center line, D acl represents the offset of the trajectory from the center line; W dtfsst represents the weight of the distance between the trajectory end point and the front static obstacle, D acl represents the distance between the trajectory end point and the front static obstacle.

[0208] In the embodiment of the application, one or more safe, comfortable and feasible lane changing points conforming to the kinematic principle can be given according to the position of dynamic or static obstacles between the current lane, the target lane and the parking position, the future predicted trajectory, the current position of the vehicle, the sampling according to time, speed, acceleration, lateral and longitudinal positions in the frenet coordinate system. Based on the trajectory information of the obstacles, the target parking position near the parking position is given for each lane changing point, and finally a smooth lane changing trajectory is calculated by using a quadratic programming solver, and a target lane changing and parking trajectory is obtained. In this way, a smooth, safe, comfortable and executable trajectory can be generated in a dynamic environment in near real time, which is suitable for structured or semi-structured roads in various urban and non-urban areas, and can ensure the effect of finally completing the lane changing and parking.

[0209] It should be noted that in this document, the terms "comprise", "comprise", or any other variant thereof are intended to cover non-exclusive inclusion, so that a process, method, article or system including a series of elements includes not only those elements, but also other elements not explicitly listed, or inherent to such a process, method, article or system. Without more limitations, the element defined by the statement "comprises a" does not exclude the presence of another identical element in the process, method, article or system including the element.

[0210] The above-mentioned embodiment numbers of the present application are only for description, and do not represent the advantages and disadvantages of the embodiments.

[0211] Through the above description of the embodiments, those skilled in the art can clearly understand that the above-mentioned embodiment methods can be realized by means of software and the necessary general hardware platform, of course, they can also be realized by hardware, but in many cases the former is a better embodiment. Based on such understanding, the technical solutions of the present application can be embodied in the form of a software product, which is stored in a storage medium (such as a ROM / RAM, a magnetic disk, or an optical disk) as described above, and includes a plurality of instructions for causing an end device (which can be a mobile phone, a computer, a server, a controller, or a network device, etc.) to execute the methods described in the various embodiments of the present application.

[0212] The above is only the preferred embodiment of the present application, and does not limit the patent scope of the present application, and any equivalent structure or equivalent flow transformation made by using the content of the specification and drawings, or directly or indirectly applied to other related technical fields, are also included in the patent protection scope of the present application.

Claims

1. A lane keeping trajectory planning method, characterized in that, The method comprises the following steps: S1: Projecting the predicted trajectory of static obstacles and / or dynamic obstacles according to the position of the ego vehicle, the target lane, and the end position to obtain obstacles affecting lane changing; S2: At each preset time, obtaining one or more lane changing points according to the position of the obstacles and the speed performance of the ego vehicle; S3: Calculating the corresponding target parking position for the lane changing points according to the lane changing parameters, wherein the lane changing parameters include one or more of the end position, the position of the static obstacles, the brake comfort, and the no-parking area; S4: In the frenet coordinate system, establishing constraints of the obstacles and the road edge along the s direction, and considering the trajectory continuity and the lateral offset and its derivative constraints, calculating the smooth lane changing trajectory using a quadratic programming solver; S5: Calculating the corresponding cost value for multiple lane changing trajectories according to the cost function, and confirming the lane changing trajectory corresponding to the minimum cost value as the target curb parking trajectory; Wherein, the step S2 comprises: At each preset time, obtaining one or more lane changing points according to s, t, and v1; Wherein, t represents each preset time, s represents the position of the ego vehicle in the target lane in the frenet coordinate system, and v1 represents the speed of the ego vehicle; the lane changing points include one or more of the acceleration point, the deceleration point, the front vehicle yielding point, the rear vehicle overtaking point, and the window middle point; When the target lane has static obstacles, a lane changing point that avoids the static obstacles is generated; When the ego vehicle approaches a fork, a lane changing point that successfully changes lanes is generated; Wherein, the step S3 comprises: If the lane changing parameters include the end position, the target parking position is located at a position that the ego vehicle can reach at the current speed; If the lane changing parameters include the position of the static obstacles, the target parking position is at a safe distance from the position of the static obstacles; If the lane changing parameters include the brake comfort, the target parking position is at a position that meets the brake comfort; If the lane changing parameters include the no-parking area, the target parking position is not in the no-parking area.

2. The off-tracking trajectory planning method according to claim 1, characterized in that, The step S1 comprises: B1: Dividing multiple preset times within a preset time, and at each preset time, projecting the predicted trajectory of static obstacles and / or dynamic obstacles to obtain the position of the obstacles affecting lane changing according to the position of the ego vehicle and the target lane.

3. The off-tracking trajectory planning method according to claim 2, characterized in that, The step B1 comprises: Confirming the region of interest; wherein the region of interest is the area from the right side of the ego vehicle to the two side edges of the target lane where the ego vehicle is completely located; Confirming the position of the obstacles on the target lane according to the region of interest; Wherein, the left edge of the obstacle is located in the region of interest, and the left boundary of the additional margin of the obstacle is also located in the region of interest.

4. The off-tracking trajectory planning method of claim 1, wherein, The step S4 comprises: Sampling along the s direction with a interval Δs to calculate the feasible region of each lane changing point; Determining the lateral offset and its derivative according to the speed of the ego vehicle; The continuity of the trajectory is calculated by the following formula: l" i+1 = l" i + l'" i Δs where i denotes the index of the trajectory point, l ref represents the 1st order continuous reference line of lateral offset, l represents the lateral offset of the trajectory, l' represents the 1st order derivative of the lateral offset, l" represents the 2nd order derivative of the lateral offset, l'" represents the 3rd order derivative of the lateral offset, Δs represents the longitudinal displacement of the trajectory, Δs 2 represents the square of the longitudinal displacement of the trajectory, Δs 3 represents the cube of the longitudinal displacement of the trajectory; The optimization objective function used in the quadratic programming solver is: Where w1, w2, w3, and w4 represent the weights of each objective function, and J represents the error value of the trajectory. The smaller the error value, the better the smoothness of the trajectory.

5. The off-tracking trajectory planning method of claim 1, wherein, The cost function is: C t = W yaw * D yaw + W lat * D lat + W lon * D lon + W acl * D acl + W dtfsst * D dtfsst In the formula, C t represents the value of the trajectory, the smaller the value of the trajectory, the better the effect of the trajectory; W yaw represents the weight of the angle offset, D yaw represents the angle offset; W lat weight representing lateral offset, D lat lateral offset; W lon weight representing longitudinal offset, D lon longitudinal offset; W acl weight representing offset from centerline, D acl offset of trajectory from centerline; W dtfsst weight representing distance of trajectory end to front stationary obstacle, D dtfsst distance of trajectory end to front stationary obstacle.

6. A lane keeping trajectory planning device characterized by comprising: The pull-over parking trajectory planning device includes: A first detection unit is configured to project predicted trajectories of static and / or dynamic obstacles based on the vehicle position, the target lane, and the destination position to obtain obstacles that affect lane change; a second detection unit, configured to obtain one or more lane change points at each preset time according to the vehicle's speed performance and the location of the obstacle; an acquiring unit, configured to calculate a corresponding target parking position for the lane change point based on lane change parameters, wherein the lane change parameters include one or more of an end position, a position of a static obstacle, a braking comfort level, and a no-parking zone; The first generation unit is used to establish constraints on obstacles and road edges along the s direction in the Frenet coordinate system, and consider trajectory continuity and lateral offset and its derivative constraints, and use a quadratic programming solver to calculate a smooth lane change trajectory; a second generating unit, configured to calculate corresponding cost values ​​for the plurality of lane change trajectories according to the cost function, and identify the lane change trajectory corresponding to the minimum cost value as the target pull-over parking trajectory; Wherein, the second detection unit is further used for: At each preset moment, one or more lane change points are obtained according to s, t, and v1; Where t represents each preset time, s represents the position of the vehicle in the target lane in the Frenet coordinate system, and v1 represents the vehicle speed. The lane change point includes one or more of the following: acceleration point, deceleration point, yielding point, overtaking point, and window midpoint. When there is a static obstacle in the target lane, generating a lane change point to bypass the static obstacle; When the ego vehicle approaches a fork in the road, a lane change point is generated where the lane change is successful; The acquisition unit is further configured to: If the lane change parameter includes an end position, the target parking position is located at a position that the vehicle can reach at the current speed; If the lane change parameter includes the position of a static obstacle, the target parking position is at a safe distance from the position of the static obstacle; If the lane change parameter includes braking comfort, the target parking position is at a position that meets the braking comfort; If the lane change parameter includes a no-parking zone, the target parking position is not within the no-parking zone.

7. A lane keeping trajectory planning device characterized by comprising: include: memory and processor; The memory is used to store programs; The processor is configured to execute the program to implement each step of the pull-over parking trajectory planning method as described in any one of claims 1 to 5.

8. A computer-readable storage medium having stored thereon a computer program, characterized in that, When the computer program is executed by a processor, the steps of the pull-over parking trajectory planning method according to any one of claims 1 to 5 are implemented.

Images