A method and system for intersection left-turn vehicle trajectory quadratic programming

Abstract

The application belongs to the technical field of traffic safety and intelligent traffic, and particularly relates to a left-turn vehicle passing trajectory quadratic programming method and system for an intersection, which is based on the geometric relationship of the intersection to establish a first local coordinate system to generate an initial left-turn trajectory; vehicle state and environmental information are acquired in real time during vehicle driving, and the current position, speed direction and expected departure position of the vehicle are determined when the trigger condition is met; a second local coordinate system is constructed with the current position of the vehicle, the current position and the expected departure position are equivalent to a virtual entrance lane and a virtual exit lane, a third-order Bezier quadratic programming trajectory is generated through parameter mapping; and finally the vehicle is controlled to pass according to the quadratic trajectory or the initial trajectory. The application solves the problem that a one-time static trajectory is difficult to adapt to real-time interference of a left-turn vehicle in a dynamic intersection environment, realizes real-time reprogramming of the remaining trajectory, and enables the vehicle to still have continuous, smooth and safe passing capacity in a complex environment.

Description

Technical Field

[0001] This application belongs to the field of traffic safety and intelligent transportation technology, and specifically relates to a method and system for secondary planning of the trajectory of vehicles turning left at intersections. Background Technology

[0002] With the rapid development of Intelligent Transportation Systems (ITS) and autonomous driving technologies, the issue of safe and efficient vehicle passage in complex urban traffic environments has become increasingly prominent. Urban intersections, as one of the most complex and conflict-concentrated key nodes in road traffic networks, directly impact the overall network's capacity and safety. Left-turn traffic, in particular, involves the convergence and weaving of traffic flows from multiple directions, easily creating conflict zones and making it a high-risk scenario for traffic accidents. Therefore, planning reasonable trajectories for left-turning vehicles to improve throughput and smoothness while meeting safety constraints has become an important research direction in the fields of intelligent driving and intelligent transportation.

[0003] In existing technologies, intersection trajectory planning methods are mostly based on regular lane line designs or pre-built static high-precision maps. By pre-setting waypoints or reference lines, they provide vehicles with fixed turning trajectories. These methods typically assume that the road structure is clear, lane lines are well-visible, and traffic participants behave relatively normally. Under these premises, vehicles can perform path tracking control along a predetermined trajectory, basically meeting the traffic needs of general scenarios.

[0004] However, in real-world urban road environments, road structures and traffic conditions are often far more complex and variable. On the one hand, traditional trajectory planning methods based on regular lane lines or static path points struggle to simultaneously consider multiple factors such as road geometry, real-time traffic flow, and vehicle dynamics and kinematic constraints. This results in left-turn trajectories that are not smooth enough in terms of curvature changes, acceleration, and lateral offset, leading to higher vehicle control costs and poorer ride comfort. On the other hand, when there are issues such as blurred lane lines, worn road markings, unprotected left-turn signal control, and highly uncertain behavior of non-motorized vehicles and pedestrians, the aforementioned methods show significant deficiencies in adaptability and robustness to complex intersection environments. This can easily lead to a mismatch between the trajectory and the actual feasible passage space, thereby causing potential safety hazards.

[0005] To improve the continuity and smoothness of trajectories, Bézier curves are widely used in vehicle trajectory planning due to their excellent geometric properties and differentiability. Third-order Bézier curves, in particular, allow for flexible adjustment of the trajectory shape by controlling the endpoints and control points. This ensures continuity in position and velocity while also meeting vehicle steering constraints and comfort requirements, making them a common tool for generating trajectories such as turns and lane changes.

[0006] Most existing trajectory planning methods based on Bézier curves adopt the approach of "one-time trajectory generation," that is, given the road structure and the topological relationship between entrance and exit lanes, a so-called "optimal" turning trajectory is directly calculated, and this trajectory is not updated or corrected during the actual vehicle execution. For example, the invention patent "A Vehicle Unified Path Planning Method Based on the Positional Relationship of Intersection Entrance and Exit Lanes" (authorization announcement number: CN115762133B) discloses an intersection turning trajectory planning technology based on Bézier curves. By using the spatial positional relationship between the entrance and exit lanes of an intersection, the vehicle turning trajectory is planned uniformly, and a relatively reasonable turning path can be generated in one go when the static road information is known.

[0007] However, the aforementioned existing technologies have the following shortcomings: First, these methods are usually based on static environment assumptions, ignoring the dynamic interference introduced by surrounding traffic participants (such as vehicles in front, oncoming vehicles, non-motorized vehicles, and pedestrians) during the actual driving process. When temporary stops, sudden decelerations, or pedestrians suddenly crossing occur at intersections, if the vehicle still strictly follows the initial planned trajectory, it is prone to conflicts with other traffic participants or insufficient safe distances. Second, the trajectory generated in one go cannot reflect real-time traffic information such as changes in traffic light phases and dynamic changes in passable areas, causing the trajectory to no longer be the optimal solution for safety or efficiency at certain times. Third, when the vehicle deviates from the original trajectory due to avoidance, deceleration, or other operations during execution, existing methods usually lack a mechanism for replanning and reconnecting the trajectory in the deviated state, which can easily lead to problems such as trajectory discontinuity and sudden curvature changes, affecting the vehicle's control stability and ride comfort.

[0008] Therefore, in complex and dynamic left-turn scenarios at intersections, relying solely on a one-time Bézier curve trajectory generation strategy is insufficient to meet the comprehensive requirements of autonomous vehicles for safety, smoothness, and robustness. There is an urgent need to propose a secondary trajectory planning method for left-turning vehicles. After initial trajectory planning, this method should combine the vehicle's current state information and real-time changes in the intersection environment to re-plan, correct, and locally optimize the original trajectory, achieving dynamic updates and smooth, continuous connection of the trajectory. This will improve the safety, traffic efficiency, and control stability of left-turning vehicles passing through intersections. Summary of the Invention

[0009] To address the shortcomings of existing technologies that rely on static, one-time trajectory generation to handle dynamic traffic interference at intersections, fail to reflect changes in signal phase and passable areas in a timely manner, and lack continuous replanning capabilities after vehicles deviate from their original trajectories, leading to technical problems such as trajectory mismatch, insufficient safety distance, and unstable control for left-turning vehicles during actual passage, this application provides a method and system for secondary trajectory planning for left-turning vehicles at intersections. The technical solution aims to resolve situations where left-turning vehicles cannot safely pass along their planned trajectories due to dynamic interference from traffic participants. By performing real-time replanning, correction, and local optimization of the remaining unexecuted trajectories based on the initial trajectory planning, this ensures that vehicles can still obtain continuous, smooth, and feasible trajectories with safety margins even in complex intersection environments. This enables left-turning vehicles to smoothly exit the intersection, improving driving safety, traffic efficiency, and control stability.

[0010] On the one hand, this application provides a method for quadratic planning of the trajectory of left-turning vehicles at intersections, the method comprising:

[0011] Step 1: Based on the geometric positional relationship between the intersection's approach lanes and exit lanes and the driving constraints of left-turning vehicles, establish a first local coordinate system in the intersection area, and generate the initial left-turning trajectory of left-turning vehicles passing through the intersection under the first local coordinate system.

[0012] Step 2: During the process of the left-turning vehicle traveling along the initial left-turning trajectory, the status information of the left-turning vehicle and the intersection environment information are obtained in real time. When it is determined that the trajectory secondary planning trigger condition is met based on the status information and environment information, the current position and speed direction of the left-turning vehicle are obtained, and the updated expected departure position is determined based on the current traffic demand of the intersection.

[0013] Step 3: Establish a second local coordinate system with the current position of the left-turning vehicle in Step 2 as the origin and the expected speed direction of the left-turning vehicle at the trigger time as the Y-axis. Equivalently represent the current position and its driving direction as the virtual entrance lane of the quadratic planning trajectory, and equivalently represent the updated expected departure position and its corresponding driving direction as the virtual exit lane of the quadratic planning trajectory. Determine the position parameters based on the positional relationship between the virtual entrance lane and the virtual exit lane in the second local coordinate system.

[0014] Step 4: Based on the position parameters determined in Step 3 and the preset parameter mapping relationship, determine the control point distance parameters of the third-order Bézier curve, and construct the quadratic programming third-order Bézier trajectory of the left-turning vehicle in the second local coordinate system with the current position of the left-turning vehicle and the updated expected departure position as the endpoints.

[0015] Step 5: Control the left-turning vehicle according to the quadratic programming third-order Bezier trajectory generated in Step 4, so that the left-turning vehicle passes through the intersection along the quadratic programming third-order Bezier trajectory. If the quadratic programming trigger condition in Step 2 is not met, control the left-turning vehicle to pass through the intersection along the initial left-turning trajectory described in Step 1.

[0016] In a preferred implementation, step 1 further includes:

[0017] Step 1.1: Obtain the geometric information of the intersection's approach and exit lanes;

[0018] Step 1.2: Taking the intersection approach lane as the reference, set the midpoint of the parking line of the approach lane as the origin O of the first local coordinate system XOY, set the extension line of the parking line as the X-axis, and set the direction perpendicular to the parking line and pointing into the intersection as the Y-axis. Establish the first local coordinate system for left turn trajectory planning in the intersection area.

[0019] Step 1.3: In the first local coordinate system XOY, based on the relative position and direction of the centerline of the exit lane and the centerline of the inlet lane, determine the position parameters of the exit lane relative to the inlet lane. Among them, the horizontal distance along the X-axis represents the lateral distance parameter w, the vertical distance along the Y-axis represents the longitudinal distance parameter h, and the angle between the direction of the centerline of the exit lane and the positive direction of the Y-axis represents the direction angle parameter α.

[0020] Step 1.4: Based on the vehicle model parameters and driving conditions of the vehicle turning left, obtain the driving constraints that affect the trajectory planning;

[0021] Step 1.5: Based on the position parameters w, h, and α obtained in Step 1.3, and the driving constraints in Step 1.4, the control point distance parameters of the third-order Bézier curve are obtained through a pre-established parameter mapping relationship or regression model. and ,Depend on , Determine the coordinates of each control point of the third-order Bézier curve in the first local coordinate system XOY;

[0022] Step 1.6: In the first local coordinate system XOY, take the reference starting point of the left-turning vehicle at the stop line of the entrance lane as the starting point of the third-order Bézier curve, and take the reference ending point of the center line of the exit lane at the exit position of the intersection as the ending point of the third-order Bézier curve. Combine the control point coordinates determined in Step 1.5 to construct the third-order Bézier curve, and use the third-order Bézier curve as the initial left-turning trajectory of the left-turning vehicle through the intersection.

[0023] In a preferred implementation, step 2 further includes:

[0024] Step 2.1: While the left-turning vehicle is traveling along the initial left-turning trajectory, acquire the status information of the left-turning vehicle and the intersection environment information in real time;

[0025] Step 2.2: Based on the status information of the left-turning vehicle, the intersection environment information, and the initial left-turning trajectory, predict the spatiotemporal relationship between the left-turning vehicle and other traffic participants when the left-turning vehicle passes through the preset conflict area along the initial left-turning trajectory. When it is determined that the left-turning vehicle will cause a conflict in the preset conflict area or insufficient safety distance under the preset safety distance and / or preset safety time interval, determine that the trajectory secondary planning trigger condition is met.

[0026] Step 2.3: If the conditions for triggering secondary trajectory planning are met as determined in Step 2.2, based on the current traffic demand at the intersection and the priority traffic objects, and based on the current position and current speed direction of the left-turning vehicle along the initial left-turning trajectory, calculate the speed direction adjustment required to avoid oncoming straight-ahead vehicles or right-hand straight-ahead vehicles, and obtain the desired speed direction for secondary planning.

[0027] In the preferred implementation, step 3 further includes: Step 3.1: Obtain the current position coordinates of the left-turning vehicle in the first local coordinate system XOY at the trigger time. and current driving speed and direction The angle φ between the coordinate system and the positive Y-axis of the first local coordinate system is used as the starting point of the quadratic programming trajectory.

[0028] Step 3.2: Using the vehicle's current position in Step 3.1 as the origin O′, establish a second local coordinate system X′O′Y′, where the direction along the vehicle's current speed is the positive direction of the Y′ axis, and the direction perpendicular to the vehicle's current speed and pointing to the right of the vehicle is the positive direction of the X′ axis. The included angle φ is used to describe the rotational relationship between the first local coordinate system XOY and the second local coordinate system X′O′Y′.

[0029] Step 3.3: In the second local coordinate system X′O′Y′, the current position O′ of the left-turning vehicle at the time of the secondary planning trigger and its driving direction Y′ axis are equivalent to the virtual entrance lane of the secondary planning trajectory: O′ is the reference starting point of the virtual entrance lane, and the Y′ axis direction is the driving direction of the virtual entrance lane;

[0030] Step 3.4: Based on the updated expected departure position obtained in Step 2.3, mark the coordinates of this expected departure position in the first local coordinate system XOY as ( , ), by coordinate translation and rotation, the point ( , Transform from the first local coordinate system XOY to the second local coordinate system X′O′Y′ to obtain the coordinates in X′O′Y′. , By combining the direction angle of the desired departure direction with the positive Y-axis direction of the first local coordinate system and the included angle φ, the direction angle of the desired departure direction with respect to the positive Y′ axis of the second local coordinate system can be obtained. , serving as the driving direction angle of the virtual exit lane;

[0031] Step 3.5: Based on the hazard index of the priority passage objects determined in Step 2.3 , Direction adjustment amount, final speed, direction adjustment amount and the offset of the desired departure point relative to the current position determined in step 3.4. , ), calculate the vertical adjustment amount caused by yielding demand. ;

[0032] Step 3.6: In the second local coordinate system X′O′Y′, the desired departure position and its driving direction in Step 3.4 are equivalent to virtual exit lanes. The position parameters are determined according to the relative positional relationship between the virtual entrance lane and the virtual exit lane: the displacement component along the X′ axis is used as the lateral distance parameter w′ of the virtual exit lane relative to the virtual entrance lane, the displacement component along the Y′ axis is used as the longitudinal distance parameter h′ of the virtual exit lane relative to the virtual entrance lane, and the angle between the driving direction of the virtual exit lane and the positive direction of the Y′ axis is used as the direction angle parameter α′. Thus, the parameter set (w′, h′, α′) describing the geometric relationship of the quadratic programming trajectory is obtained. The obtained (w′, h′, α′) is used as the input parameter for the quadratic programming trajectory generation step, and is used for the calculation of the distance parameter of the control point of the third-order Bézier curve in Step 4 and the construction of the quadratic programming trajectory.

[0033] In a preferred implementation, step 4 further includes:

[0034] Step 4.1: Use the set of position parameters (w′, h′, α′) of the virtual exit lane relative to the virtual entrance lane and the set of vehicle driving constraint parameters determined in Step 3 as input for solving the Bézier control points in the quadratic programming.

[0035] Step 4.2: Substitute (w′, h′, α′) into the preset parameter mapping relationship or regression model to obtain the distance parameter of the first control point of the third-order Bézier curve in the quadratic programming. and synthetic distance parameters ( + );

[0036] Step 4.3: Based on the vehicle's minimum permissible turning radius, maximum lateral acceleration, and comfort threshold, and Limit or correct the amplitude;

[0037] Step 4.4: In the second local coordinate system X′O′Y′, take the current position O′ of the left-turning vehicle at the moment of triggering the curve as the starting point of the third-order Bézier curve. And the updated expected departure position E′ is used as the endpoint of the third-order Bézier curve. Travel along the virtual exit lane towards the destination. Inverse vector distance Determine the second control point The coordinates, along the Y′ axis of the virtual entrance lane from the starting point. Measure distance Determine the first control point The coordinates;

[0038] Step 4.5: Utilize , , , Four feature points are used to construct a third-order Bézier curve in the second local coordinate system, and this curve is used as the output of the quadratic planning traffic trajectory for left-turning vehicles.

[0039] In the preferred implementation, further, in step 1.1, the geometric information of the intersection approach lanes and exit lanes includes the location of the stop line of the approach lane, the direction of the center lines of the approach lanes and exit lanes, and the range of the passable area inside the intersection.

[0040] In the preferred implementation, further, in step 2.2, the trigger condition for trajectory quadratic planning is: there exists any traffic participant i that satisfies... and / or ,in, The shortest time interval between a left-turning vehicle and each traffic participant within a predefined conflict zone. For the safety time interval threshold associated with left-turning vehicles, Let be the minimum spatial distance of a vehicle to traffic participant i at the predicted location at time t. The longitudinal safety distance threshold associated with left-turning vehicles;

[0041] In step 2.3, the speed and direction adjustment required for a left-turning vehicle to avoid oncoming or right-hand traffic. for:

[0042] ;

[0043] In the formula: The symbol indicates the direction of deflection for vehicles turning left; It is a saturation function; This represents the magnitude of the speed and direction adjustment for left-turning vehicles at the current moment. Adjust the amplitude in the preset maximum direction.

[0044] In the preferred implementation, further, in step 3.4, the desired speed direction of the left-turning vehicle at the trigger moment... for:

[0045] + ;

[0046] In the formula: The current speed of the vehicle turning left And the angle between its direction of travel and the positive direction of the Y-axis; The amount of speed and direction adjustment required for left-turning vehicles to avoid oncoming or right-hand vehicles going straight.

[0047] The angle parameter between the centerline direction of the exit lane and the desired speed direction of left-turning vehicles for:

[0048] ;

[0049] In the formula: The angle between the direction of the centerline of the exit channel in the first local coordinate system XOY and the positive direction of the Y-axis; The desired speed and direction of the left-turning vehicle at the moment of triggering;

[0050] In step 3.5, the longitudinal adjustment amount for left-turning vehicles. for:

[0051] ;

[0052] In the formula, The symbol indicates the direction of deflection for vehicles turning left; For offline calibration, the longitudinal adjustment ratio coefficient; The risk level index for priority passage objects.

[0053] In the preferred implementation, further, in step 3.6, regarding the left-turning vehicle yielding to the right-hand through vehicle, the expected displacement vector of the departure point relative to the virtual approach lane is:

[0054] ;

[0055] In the formula: Corresponding to the X′O′Y′ coordinate system ; Corresponding to the X′O′Y′ coordinate system ; For left-turning vehicles, the longitudinal adjustment amount is used. , The coordinates of the vehicle's current position in the first coordinate system at the moment of triggering;

[0056] For left-turning vehicles to yield to oncoming straight-ahead vehicles, the desired displacement vector of the departure point relative to the virtual approach lane is:

[0057] ;

[0058] Lateral distance parameter between virtual exit lane and virtual entrance lane for:

[0059] ;

[0060] Longitudinal distance parameter between virtual exit lane and virtual entrance lane for:

[0061] .

[0062] On the other hand, this application also provides a secondary planning system for the trajectory of left-turning vehicles at intersections, the system comprising:

[0063] The initial trajectory generation module is used to establish a first local coordinate system in the intersection area based on the geometric positional relationship between the intersection entrance lane and the exit lane and the driving constraints of left-turning vehicles, and to generate the initial left-turning trajectory of left-turning vehicles passing through the intersection in the first local coordinate system.

[0064] The trigger determination and departure update module is used to acquire the status information of the left-turning vehicle and the intersection environment information in real time during the left-turning vehicle's journey along the initial left-turning traffic trajectory. Based on the status information and environment information, it determines whether the trajectory secondary planning trigger condition is met. When the trigger condition is met, it acquires the current position and speed direction of the left-turning vehicle and determines the updated expected departure position based on the current traffic demand of the intersection.

[0065] The second coordinate system and position parameter determination module is used to establish a second local coordinate system with the current position of the left-turning vehicle obtained by the trigger judgment and departure update module as the origin and the expected speed direction of the left-turning vehicle at the trigger time as the Y-axis. The current position and its driving direction are equivalent to the virtual entrance lane of the quadratic planning trajectory, and the updated expected departure position and its corresponding driving direction are equivalent to the virtual exit lane of the quadratic planning trajectory. The position parameters of the quadratic planning are determined according to the relative positional relationship between the virtual entrance lane and the virtual exit lane under the second local coordinate system.

[0066] The quadratic programming Bézier curve trajectory generation module is used to determine the control point distance parameters of the quadratic programming third-order Bézier curve based on the position parameters determined by the second coordinate system and the position parameter determination module, as well as the preset parameter mapping relationship. It also constructs the quadratic programming third-order Bézier curve trajectory of the left-turning vehicle in the second local coordinate system with the current position of the left-turning vehicle and the updated expected departure position as the endpoints.

[0067] The trajectory tracking control module is used to control the driving of left-turning vehicles according to the quadratic programming Bézier trajectory generated by the quadratic programming Bézier trajectory generation module, so that the left-turning vehicles pass through the intersection along the quadratic programming Bézier curve trajectory, and control the left-turning vehicles to pass through the intersection along the initial left-turning traffic trajectory if the trajectory quadratic programming trigger condition is not met.

[0068] The beneficial effects of this application are:

[0069] First, the quadratic planning method for the trajectory of left-turning vehicles at intersections in this application overcomes the problems of existing one-time trajectory generation methods, such as difficulty in adapting to dynamic traffic interference at intersections, inability to reflect real-time changes in passable areas, and lack of continuous trajectory connection after vehicles deviate from the original trajectory, by introducing a quadratic planning mechanism for the trajectory of left-turning vehicles at intersections. By generating an initial left-turn trajectory under the geometric constraints of the approach and exit lanes, and determining in real time whether to trigger quadratic planning based on vehicle status and environmental information during vehicle travel, the system can actively identify dynamic events such as deceleration of the vehicle in front, approaching oncoming vehicles, and sudden entry of non-motorized vehicles or pedestrians into the conflict zone during travel. When the triggering conditions are met, the method can establish a second local coordinate system at the vehicle's current position, construct virtual approach and exit lanes, and use third-order Bézier curves to generate a continuous, smooth, and vehicle dynamics-constrained updated trajectory in real time, achieving a natural connection between the initial planned trajectory and the re-planned trajectory. Through the above technical solutions, the present invention can effectively improve the safety, traffic efficiency and control stability of left-turning vehicles in complex intersection environments, reduce the potential conflict risks caused by trajectory mismatch, and at the same time improve the flexibility of trajectory generation and environmental adaptability, thereby enhancing the robustness and practicality of autonomous vehicles in intersection left-turning scenarios.

[0070] Secondly, in the preferred implementation, this application, through the technical solution described in step 1, constructs a unified, standardized first local coordinate system in the intersection area that is highly matched with the generation of the vehicle's left-turn path, enabling the spatial relationship between the approach lane and the exit lane to be accurately expressed in the form of structured parameters. By transforming the geometric relationship between the exit lane and the approach lane into three positional parameters—lateral distance w, longitudinal distance h, and direction angle α—and combining them with driving constraints such as vehicle size and minimum turning radius, adaptive modeling for different intersection structures and different vehicle operating conditions can be achieved. Based on this, the distance parameters of the control points of the third-order Bézier curve are obtained through a preset parameter mapping relationship or a trained regression model. and This not only improves the automation level of control point generation but also ensures smooth trajectory curvature changes and meets vehicle dynamics feasibility. The final constructed initial left-turn Bezier trajectory accurately fits the intersection geometry and vehicle left-turn requirements, exhibiting good continuity, controllability, and smoothness, providing a stable and reliable foundation for subsequent trajectory tracking and secondary planning.

[0071] Third, in the preferred implementation, this application, through the technical solution in step 2, can achieve a high degree of dynamic perception of the vehicle status and the intersection environment throughout the entire process of a left-turning vehicle traveling along the initial trajectory, and utilizes a safe time interval Δt. i Minimum distance D in space i Two types of indicators are used to construct a dual-constraint conflict risk quantification model, enabling the system to more accurately identify potential collision threats posed to left-turning vehicles by oncoming straight-ahead vehicles, lateral vehicles, non-motorized vehicles, or pedestrians. This risk assessment method not only provides early warnings at the early stages when traffic participants approach the predicted conflict zone, but also calculates the desired departure position and adjustment direction in real time based on the vehicle's current position and speed direction, ensuring the timeliness and reliability of trajectory secondary planning. When insufficient safety margin is detected, the system can automatically generate reasonable speed and direction adjustments based on the current speed and steering requirements, allowing left-turning vehicles to proactively avoid potential conflict objects and adjust the upcoming trajectory planning. This enhances the proactive safety capabilities and behavioral autonomy of left-turning vehicles in the dynamic environment of complex intersections, reducing the conflict risk caused by delayed trajectory adjustments.

[0072] Fourth, in the preferred implementation, this application further utilizes the technical solution in step 3 to dynamically reconstruct the second local coordinate system based on the vehicle's real-time position and velocity direction when triggering secondary planning. This allows the trajectory replanning to no longer be limited by the initial approach lane direction, thereby achieving natural alignment between the vehicle's current posture and the planned future trajectory. By equating the vehicle's current position to the virtual approach lane starting point in the second local coordinate system, and automatically calculating the desired velocity direction angle based on the vehicle's avoidance requirements for straight-ahead or lateral traffic participants, this process is further refined. Lateral offset With longitudinal adjustment amount The system can generate new desired departure positions that meet safety avoidance requirements in real time. Furthermore, this step maps the aforementioned offsets to the lateral distance parameter w′ and longitudinal distance parameter h′ of the virtual exit lane, achieving a geometric and parameterized expression of the future planning space. This allows vehicles to quickly construct new passable areas and virtual road topologies based on the dynamic environment.

[0073] Fifth, in the preferred implementation, this application further parameterizes the geometric relationship between the dynamically constructed virtual exit lane and virtual entrance lane through the technical solution in step 4, and combines it with the vehicle's driving constraints as input for solving the quadratic programming Bézier control points, enabling the quadratic programming trajectory to maintain structured and controllable generation characteristics under real-time environmental conditions. By introducing a preset parameter mapping relationship or regression model, the virtual road parameters w′, h′, α′ are transformed into control distance parameters of the third-order Bézier curve. + This not only improves the automation level of control point acquisition but also ensures that changes in the curvature, length, and direction of the trajectory can adapt to the vehicle's dynamic capabilities and safety requirements. Simultaneously, this step combines the vehicle's minimum turning radius, maximum lateral acceleration, and comfort threshold to... and Constraint modifications are made to ensure that the generated quadratic Bézier curve meets real-time obstacle avoidance requirements while maintaining smoothness and feasibility. Finally, in the second local coordinate system, with the vehicle's current position as the starting point and the desired departure position as the ending point, a four-control-point third-order Bézier curve is constructed. This curve can reflect the dynamic passable space within the intersection in real time, achieving simultaneous optimization of trajectory direction, curvature, and path shape. This allows left-turning vehicles to obtain a safe, continuous, and highly controllable quadratic planning trajectory even under sudden traffic disturbances.

[0074] Sixth, the intersection left-turn vehicle trajectory secondary planning system of this application integrates initial trajectory planning, dynamic trigger judgment, local coordinate system reconstruction, parameterized Bézier curve replanning and trajectory tracking control into one system, realizing real-time updating of the traffic trajectory and safe and smooth passage of left-turn vehicles in complex dynamic environments, greatly improving the autonomous decision-making ability, safety and environmental adaptability of intersection traffic. Attached Figure Description

[0075] Figure 1 This is a flowchart of a method for secondary planning of the trajectory of left-turning vehicles at intersections, according to an embodiment of the present invention.

[0076] Figure 2 This is a schematic diagram illustrating the trajectory adjustment of left-turning vehicles at an intersection according to an embodiment of the present invention.

[0077] Figure 3 This is a schematic diagram of the position parameters of the initial left-turn trajectory when a left-turning vehicle yields to a vehicle going straight on the right, according to an embodiment of the present invention.

[0078] Figure 4 This is a schematic diagram of the position parameter coordinate system transformation of the secondary planning trajectory of a left-turning vehicle at an intersection in a scenario where a left-turning vehicle yields to a vehicle going straight on the right, as described in an embodiment of the present invention.

[0079] Figure 5 This is a schematic diagram of the expected departure position coordinate system transformation of the secondary planning trajectory of a left-turning vehicle at an intersection in a scenario where a left-turning vehicle yields to a vehicle going straight on the right, according to an embodiment of the present invention.

[0080] Figure 6 A schematic diagram illustrating the relationship between the secondary planning trajectory parameters of a left-turning vehicle at an intersection in a scenario where a left-turning vehicle yields to a vehicle going straight on the right, as described in an embodiment of the present invention.

[0081] Figure 7 A comparison diagram of the actual trajectory and simulated trajectory of a left-turning vehicle in a scenario where a left-turning vehicle yields to a vehicle going straight on the right, according to an embodiment of the present invention.

[0082] Figure 8 This is a comparison chart of the actual speed and simulated speed of a left-turning vehicle in a scenario where a left-turning vehicle yields to a vehicle going straight on the right, according to an embodiment of the present invention.

[0083] Figure 9 The actual trajectory and simulated trajectory of a straight-going vehicle in the scenario where a left-turning vehicle yields to a right-hand straight-going vehicle, according to an embodiment of the present invention;

[0084] Figure 10 This is a comparison chart of the actual speed and simulated speed of a vehicle going straight in a scenario where a left-turning vehicle yields to a vehicle going straight on the right, according to an embodiment of the present invention. Detailed Implementation

[0085] To enable those skilled in the art to better understand the technical solutions of this application, the following will provide a more detailed description of this application in conjunction with the accompanying drawings and embodiments.

[0086] The directional terms such as above, below, left, right, front, and back used in this application are based on the positional relationships shown in the attached drawings. Different attached drawings may result in different positional relationships, therefore they should not be interpreted as limitations on the scope of protection.

[0087] In this application, the terms "installation," "connection," "interlocking," "linking," and "fixing," etc., should be interpreted broadly. For example, they can refer to a fixed connection, a detachable connection, an integral connection, a mechanical connection, an electrical connection, or a connection that allows communication between components. They can also refer to a direct connection or an indirect connection through an intermediate medium. They can refer to the internal connection of two components or the interaction between two components. For those skilled in the art, the specific meaning of the above terms in this application can be understood according to the specific circumstances.

[0088] This invention describes a method and system for secondary planning of the trajectory of vehicles turning left at an intersection. Based on the initial trajectory planning, the system can replan and correct the left-turn trajectory according to the vehicle status or the dynamic environment of the intersection, thereby ensuring that the vehicle can smoothly exit the intersection.

[0089] As per the instruction manual Figure 1 The quadratic planning method for the trajectory of left-turning vehicles at intersections according to the present invention includes:

[0090] Step 1: Based on the geometric positional relationship between the intersection's approach lanes and exit lanes and the driving constraints of left-turning vehicles, establish a first local coordinate system in the intersection area, and generate the initial left-turning trajectory of left-turning vehicles passing through the intersection under the first local coordinate system.

[0091] The purpose of step 1 is to obtain a baseline left-turn trajectory that satisfies the intersection geometry and vehicle driving constraints, providing a reference trajectory and initial departure target for subsequent determination of whether secondary planning is needed and for trajectory correction.

[0092] As per the instruction manual Figure 2 Within the limited space of an intersection, in order to avoid conflict with other vehicles going straight, drivers usually make certain adjustments to their original left-turn trajectory, thereby giving priority to other vehicles in terms of space. Figure 1 In the diagram, green vehicles represent left-turning vehicles, and yellow vehicles represent straight-going vehicles. The black dashed line in the green vehicle trajectory represents the left-turn trajectory obtained according to the initial plan, while the blue dashed line represents the left-turn secondary planning trajectory obtained using the method of this invention after meeting the yield requirement. Figure 1(a) illustrates a scenario where a left-turning vehicle yields to an oncoming straight-ahead vehicle. After entering the intersection from the approach lane, the original left-turn trajectory of the left-turning vehicle clashes with the trajectory of the oncoming straight-ahead vehicle in the center of the intersection. To allow the oncoming straight-ahead vehicle to pass this clash point first, the driver increases the turning radius of the left turn by veering the turning path outwards from the intersection. This involves appropriately shifting the end point of the left-turn trajectory backwards and increasing the longitudinal distance between the start and end points of the trajectory. This extends the travel distance of the left-turning vehicle before reaching the clash point and delays its entry into the clash area, thus providing sufficient safe passage space for the oncoming straight-ahead vehicle. Figure 1 (b) illustrates a scenario where a left-turning vehicle yields to a vehicle going straight on its right. In this case, a merging conflict point exists between the left-turning vehicle and the vehicle going straight from its right within the intersection. To allow the right-hand vehicle to pass through this merging conflict point first, the driver narrows the turning path inwards from the intersection, reducing the turning radius of the left turn. This involves appropriately moving the end point of the left-turn trajectory forward and shortening the longitudinal distance between the start and end points of the trajectory. This allows the left-turning vehicle to complete the turn earlier and reduce its occupation of the merging area before approaching the conflict point, thus reserving a merging passage for the right-hand vehicle. Therefore, it can be seen that... Figure 1 Both scenarios shown reflect the actual driving behavior of drivers who flexibly yield to vehicles going straight from different directions within the limited space of an intersection by adjusting the longitudinal positional relationship between the start and end points of the left-turn trajectory and the overall turning radius. This provides a behavioral basis and typical scenarios for subsequent secondary planning of left-turn vehicle trajectories based on changes in the longitudinal distance between the start and end points of the trajectory.

[0093] As per the instruction manual Figure 3 Specifically, step 1 includes:

[0094] Step 1.1: Obtain the geometric information of the intersection's approach and exit lanes.

[0095] The geometric information of the approach and exit lanes of an intersection, including the location of the stop line of the approach lane, the orientation of the center lines of the approach and exit lanes, and the extent of the passable area inside the intersection, is used to characterize the spatial relationship between the approach and exit lanes.

[0096] The geometric information of the approach and exit lanes of an intersection can be obtained based on a pre-constructed high-precision electronic road map and / or intersection design drawings. The electronic map or design drawings include vector elements such as lane centerlines, stop lines, curb lines, and the intersection outline. Alternatively, environmental perception of the intersection scene can be achieved using vehicle-mounted cameras, millimeter-wave radar, lidar, and roadside sensing devices. The electronic map can then be calibrated online in conjunction with vehicle positioning results to extract geometric information such as the location of the stop line at the approach lane, the direction of the lane centerlines at the approach and exit lanes, and the boundary coordinates of the passable area within the intersection. Preferably, before system deployment, the target intersection is surveyed or mapped offline to create an intersection structure database containing the aforementioned geometric information. When the vehicle executes the method of this invention, it reads the geometric information of the corresponding intersection from the intersection structure database.

[0097] Step 1.2: Using the intersection approach lane as a reference, set the midpoint of the parking line of the approach lane as the origin O of the first local coordinate system XOY, set the extension of the parking line as the X-axis, and set the direction perpendicular to the parking line and pointing into the intersection as the Y-axis. Establish the first local coordinate system for left turn trajectory planning in the intersection area.

[0098] Step 1.3: In the first local coordinate system XOY, based on the relative position and direction of the centerline of the exit lane and the centerline of the inlet lane, determine the position parameters of the exit lane relative to the inlet lane. Among them, the horizontal distance w is represented by the horizontal distance along the X-axis, the vertical distance h is represented by the vertical distance along the Y-axis, and the direction angle α is represented by the angle between the direction of the centerline of the exit lane and the positive direction of the Y-axis.

[0099] In the first local coordinate system XOY, with the midpoint O of the stop line at the entrance lane as the origin, the X-axis is arranged along the direction of the stop line, and the Y-axis is perpendicular to the stop line and points inward into the intersection. The intersection of the starting boundary line of the exit lane (the lane boundary closest to the intersection) and the center line of the exit lane is denoted as point E. The lateral distance parameter w is the projected distance on the Y-axis from point E, the intersection of the starting boundary of the exit lane and the center line of the exit lane. The longitudinal distance parameter h is the projected distance on the X-axis from point E, the intersection of the starting boundary of the exit lane and the center line of the exit lane. The direction of the entrance lane center line in the first local coordinate system XOY is approximately horizontal to the left, and the positive direction of the Y-axis is vertically upward. Therefore, the angle between the direction of the exit lane center line and the positive direction of the Y-axis represents the direction angle parameter α, which is approximately 90° for orthogonal intersections. The angle between the vehicle's current speed direction and the positive direction of the Y-axis dynamically changes with vehicle movement. The geometric positional relationship between the inlet and outlet channels is described by w, h, and α.

[0100] Step 1.4: Based on the vehicle model parameters and driving conditions of the vehicle turning left, obtain the driving constraints that affect the trajectory planning.

[0101] Driving constraints include at least the maximum allowable steering angle or minimum turning radius, the maximum allowable lateral acceleration, and preset comfort requirements, which are used to limit the curvature change and trajectory shape of the initial left-turn trajectory.

[0102] Driving constraints can be predetermined based on the vehicle's overall parameters, steering system parameters, and turning performance indicators provided by the manufacturer, combined with the speed limits and typical road surface adhesion conditions of the intersection. Specifically, the maximum allowable steering angle and / or minimum turning radius can be determined based on the wheelbase, front and rear track width, maximum front wheel steering angle, tire and suspension parameters given in the vehicle manual or model database, combined with the minimum turning radius provided by the manufacturer under standard road surface conditions such as dry asphalt. Then, the target speed of the left-turning vehicle within the intersection area is determined according to the legal speed limits or signal control requirements of the urban roads where the intersection is located. Finally, referring to vehicle dynamics stability and ride comfort evaluation standards, the maximum allowable lateral acceleration and comfort lateral acceleration thresholds are calibrated through simulation tests or road tests, thereby forming a set of driving constraint parameters for trajectory planning.

[0103] For example, taking a Class A passenger car as an example, the wheelbase is 2.7 m, the front track is 1.55 m, and the maximum front steering angle is 35°. Based on the vehicle's steering system parameters and the turning performance indicators provided by the manufacturer, the minimum turning radius of this vehicle on dry asphalt roads under low adhesion coefficient conditions can be calculated to be 5.5 m.

[0104] Considering the speed limits of the city roads where the intersection is located, it is assumed that the target speed of left-turning vehicles after entering the intersection will not exceed 10 m / s (approximately 36 km / h). To ensure lateral stability and ride comfort during the left turn, this embodiment sets the maximum permissible lateral acceleration to 3.0 m / s², and further sets the comfort lateral acceleration threshold to 1.5 m / s². When the calculated trajectory lateral acceleration exceeds 1.5 m / s², the trajectory curvature is reduced by adjusting the Bézier curve control points.

[0105] Therefore, in this embodiment, the driving constraints for left-turning vehicles include at least the minimum allowable turning radius of the vehicle. The maximum front wheel steering angle corresponding to this turning radius Maximum lateral acceleration constraint applied during trajectory planning Lateral acceleration threshold used to ensure ride comfort When generating the initial left-turn trajectory, the radius of curvature of the third-order Bézier curve is limited to no less than [a certain value]. And the lateral acceleration calculated along the trajectory does not exceed and try to control it within The location is near the vehicle, thus meeting the requirements for vehicle dynamics constraints and ride comfort.

[0106] It should be noted that a third-order Bézier curve is a smooth curve plotted using four points. The first point is the starting point. The last point is the finish line. There are two control points in the middle. , The curve must start from... Departure, at It ends, but it doesn't necessarily go through. , Instead, it was pulled into a curved shape by these two points. By... Gradually towards , and then Finally to By performing continuous linear interpolation, a smooth, differentiable curve corresponding to the 0-1 parameter t can be obtained. This can be achieved by adjusting... , The position of the curve allows for easy control of its turning degree and direction, making third-order Bézier curves suitable for representing smooth trajectories such as vehicle turns and lane changes.

[0107] Step 1.5: Based on the position parameters w, h, and α obtained in Step 1.3, and the driving constraints in Step 1.4, the control point distance parameters of the third-order Bézier curve are obtained through a pre-established parameter mapping relationship or regression model. and ,Depend on , Determine the coordinates of each control point of the third-order Bézier curve in the first local coordinate system XOY.

[0108] in, This represents the distance from the end point of the approach lane along the approach lane direction to the first control point. This indicates the distance from the end point of the exit lane along the exit lane direction to the second control point.

[0109] Specifically, before implementing this method, measured or simulated trajectory data of left-turning vehicles at intersections with different geometric structures are collected. For each trajectory, it is fitted in the first local coordinate system XOY, and the endpoint positions of the third-order Bézier curve matching the trajectory and the corresponding control point distance parameters are calculated. and Simultaneously, the corresponding inlet and outlet channel position parameters w, h, and α are recorded to construct a sample dataset.

[0110] Using w, h, and α recorded in the sample dataset as independent variables, and + As the dependent variable, regression analysis is used to establish the parameter mapping relationship. In a preferred embodiment, linear or polynomial regression can be used to obtain the following functional form:

[0111] (1)

[0112] Furthermore, the coefficients obtained from the regression are used as pre-stored model parameters. - and - The constant coefficients are obtained by least-squares regression fitting of multiple sets of measured or simulated left-turn trajectory data that satisfy the driving constraints described in step 1.4.

[0113] In actual trajectory planning, the w, h, and α obtained in step 1.3 are substituted into the above parameter mapping relationship to calculate the current intersection geometry. and And, if necessary, based on the driving constraints such as the minimum turning radius and maximum lateral acceleration given in step 1.4, [the following is applied]: , Amplitude limiting or correction is applied to ensure that the generated third-order Bézier curve satisfies vehicle driving constraints. In determining... , Then, the starting point of the left turn at the parking line of the entrance lane is taken as the starting point of the third-order Bézier curve. The endpoint is determined by the reference point of the centerline of the exit lane at the intersection exit. In the first local coordinate system XOY, along the direction of the entrance channel from the starting point Measure distance Determine the first control point From the end point along the exit lane Inverse vector distance Determine the second control point This yields the coordinates of four feature points of the third-order Bézier curve, providing control point parameters for generating the initial left-turn trajectory in step 1.6.

[0114] Step 1.6: In the first local coordinate system XOY, take the reference starting point of the left-turning vehicle at the stop line of the entrance lane as the starting point of the third-order Bézier curve, and take the reference ending point of the center line of the exit lane at the exit position of the intersection as the ending point of the third-order Bézier curve. Combine the control point coordinates determined in Step 1.5 to construct the third-order Bézier curve, and use the third-order Bézier curve as the initial left-turning trajectory of the left-turning vehicle through the intersection.

[0115] Step 2: While the left-turning vehicle is traveling along the initial left-turning trajectory, the status information of the left-turning vehicle and the intersection environment information are obtained in real time. When it is determined that the trajectory secondary planning trigger condition is met based on the status information and environment information, the current position and speed direction of the left-turning vehicle are obtained, and the updated expected departure position is determined based on the current traffic demand of the intersection.

[0116] The purpose of step 2 is to identify the moment when trajectory replanning is required in a timely manner based on changes in vehicle state and environment during actual vehicle operation, and to collect key state variables and new departure targets for replanning, so as to provide accurate input conditions for local replanning.

[0117] Specifically, step 2 includes:

[0118] Step 2.1: While the left-turning vehicle is traveling along the initial left-turning trajectory, acquire the status information of the left-turning vehicle and the intersection environment information in real time.

[0119] The status information includes at least the current position coordinates, speed, and direction of the left-turning vehicle in the first local coordinate system. The intersection environment information includes at least the position and motion status information of other traffic participants who may have a potential conflict with the left-turning vehicle within or near the intersection. Other traffic participants include at least one of the following: vehicles going straight on the right and / or oncoming vehicles going straight, non-motorized vehicles, and pedestrians.

[0120] Specifically, the vehicle positioning module and attitude sensor are used to acquire the position information of the left-turning vehicle in the first local coordinate system XOY in real time within a preset sampling period. The current speed of the vehicle turning left is obtained through vehicle speed sensors and yaw rate sensors. and the angle between its direction of travel and the positive Y-axis. Optionally, parameters such as the longitudinal acceleration, lateral acceleration, and steering angle of the vehicle turning left can also be obtained for subsequent more refined motion prediction and constraint judgment.

[0121] Using onboard sensing devices (cameras, millimeter-wave radar, lidar, etc.) and / or vehicle-to-infrastructure (V2I) communication devices, other traffic participants within and near the intersection are detected within a preset sensing range. The detected targets are then categorized into motor vehicles (including oncoming and right-hand traffic), non-motorized vehicles, and pedestrians. The location information of each traffic participant at the current moment is obtained in the first local coordinate system XOY. Driving speed and driving direction And based on the target type and driving direction, mark the set of targets that have potential conflict relationships with left-turning vehicles.

[0122] The left-turning vehicle status information and intersection environment information are recorded in the cache queue in chronological order for use in the spatiotemporal relationship prediction in step 2.2.

[0123] Step 2.2: Based on the status information of left-turning vehicles, intersection environment information, and initial left-turning trajectory, predict the spatiotemporal relationship between left-turning vehicles and other traffic participants when passing through the preset conflict area along the initial left-turning trajectory. When it is determined that, under the preset safe distance and / or preset safe time interval, the left-turning vehicle's passage along the initial left-turning trajectory will lead to a conflict in the preset conflict area or insufficient safe distance, determine that the trajectory secondary planning trigger condition is met.

[0124] Specifically, based on the intersection geometry and the initial left-turn trajectory, one or more pre-defined conflict zones are defined in the first local coordinate system XOY. To improve safety, in a preferred embodiment, the length of the pre-defined conflict zone along its respective lane centerline direction can be set to no less than twice the vehicle length plus a certain longitudinal safety margin, and it can also cover the corresponding lane width and reserve a lateral safety margin, so that any vehicle or pedestrian entering the zone is considered to have a conflict risk. In the oncoming straight-ahead scenario, the pre-defined conflict zone is a section near the intersection of the initial trajectory of the left-turning vehicle and the centerline of the oncoming straight-ahead lane; in the right-hand straight-ahead merging scenario, the pre-defined conflict zone is the area where the initial trajectory of the left-turning vehicle intersects with the centerline of the right-hand straight-ahead lane. This pre-defined conflict zone can be described in the form of a rectangle, circle, or polygon, and is parametrically represented in the XOY coordinate system through its boundary coordinates.

[0125] Assuming the left-turning vehicle travels along the initial left-turn trajectory at its current speed within a short period of time... And / or the current acceleration is approximately uniform or uniformly accelerated, based on the arc length of the left-turning vehicle reaching the boundary of each preset conflict zone entrance along the initial trajectory. Using current speed and acceleration, kinematic formulas are used to calculate the predicted time interval for left-turning vehicles to enter each preset conflict zone. .

[0126] Among them, arc length The acquisition method is as follows: In step 1, the initial left-turn trajectory of the left-turning vehicle through the intersection was represented as a third-order Bézier curve in the first local coordinate system XOY. This third-order Bézier curve is discretized by arc length, and the cumulative arc length and coordinates of each sampling point relative to the trajectory starting point are calculated and stored, forming a "trajectory parameter - arc length" lookup table. For each predefined preset conflict area, the intersection point of the initial left-turning trajectory and the entrance boundary of the preset conflict area is calculated, and the cumulative arc length of this intersection point relative to the starting point is found in the above lookup table, denoted as "entrance cumulative arc length". During the actual driving process, the current position of the left-turning vehicle at the current moment is... Projecting onto the initial left-turn traffic trajectory, and obtaining the corresponding cumulative arc length "current cumulative arc length" from the lookup table, the arc length required to travel from the current position along the initial trajectory to the boundary of the preset conflict area is defined as:

[0127] Current cumulative arc length (2)

[0128] The current speed of the vehicle turning left has been obtained in step 2.1. Assuming that left-turning vehicles travel at approximately a constant speed along their initial left-turn trajectory within a short prediction interval, a uniform speed model is adopted:

[0129] (3)

[0130] The time increment from the current moment to the boundary of the preset conflict zone entrance for left-turning vehicles is obtained from formulas (2) and (3). Further, the lower limit of the prediction time interval is obtained. :

[0131] (4)

[0132] Determine the arc length from the current position of the left-turning vehicle to the boundary of the pre-set conflict zone entrance. Next, the intersection point of the entrance and exit boundaries of the preset conflict area is calculated on the initial left-turn trajectory, and the corresponding cumulative arc lengths are denoted as follows: and Then the arc length of the preset conflict area on the left-turn trajectory is:

[0133] (5)

[0134] Therefore, the total arc length required for a left-turning vehicle to completely exit the pre-defined conflict area from its current position along the initial left-turn trajectory is:

[0135] (6)

[0136] Assuming the left-turning vehicle moves at a constant speed or with uniform acceleration along the initial left-turning trajectory within the predicted time, its displacement along the trajectory direction satisfies:

[0137] (7)

[0138] In the formula: This is the time increment starting from the current time t; The speed at the current moment; This represents the longitudinal acceleration at the current moment.

[0139] when At that time, the corresponding positive root is the time increment required for the left-turning vehicle to completely exit the preset conflict zone. If acceleration is not considered, that is, let Then there is The upper limit of the predicted time interval for left-turning vehicles to pass through the preset conflict zone is finally obtained. :

[0140] (8)

[0141] Similarly, for each traffic participant i that has a potential conflict with a left-turning vehicle, its projected position on the corresponding lane centerline or pedestrian crossing is considered as a one-dimensional moving point along that path. The distance to the preset conflict area is obtained based on the difference between its current arc length coordinates and the arc length coordinates of the preset conflict area entrance boundary on that path. Combined with current location information Speed ​​along the path (Optional acceleration) and driving direction Similar to vehicles turning left, the time increment from the current moment to entering the predetermined conflict zone boundary is calculated using kinematic formulas for uniform speed or uniform acceleration. and order Then, based on the path length corresponding to it within the preset conflict area, the time increment for completely exiting the preset conflict area is calculated. ,get Thus, the predicted time interval for its entry into the preset conflict zone is obtained. .

[0142] Longitudinal safety distance thresholds associated with left-turning vehicles and / or safety time interval threshold This can be obtained through offline calibration before system deployment. Specifically, based on the speed limits at intersections, the maximum deceleration of vehicles under typical adhesion conditions, the perception and braking response time of the driver / control system, and the minimum following distance recommended by current traffic safety regulations, the theoretical braking distance and minimum safe headway at the maximum desired speed can be calculated. A certain safety margin can then be added to these values ​​to determine the optimal braking distance. and The value of is then determined; further, collision statistics under different scenarios are analyzed through simulation and road tests. and Calibrate and optimize it to ensure safety without being overly conservative.

[0143] The predicted time intervals for left-turning vehicles and traffic participants i are obtained. and Then, the nearest time interval and minimum spatial distance between the two can be calculated only within the time interval of their intersection. Calculate the nearest time interval between the left-turning vehicle and each traffic participant within the preset conflict area:

[0144] (9)

[0145] In the engineering implementation, t can be discretely sampled at a preset time step within the aforementioned time period. For each sampling moment, based on the predicted arc length position of the left-turning vehicle along the initial left-turning trajectory and the predicted arc length position of the traffic participant along the corresponding lane centerline, its two-dimensional coordinates in the XOY coordinate system can be obtained by looking up a table. and Calculate the Euclidean distance :

[0146] (10)

[0147] and all sampling times The minimum value in the range is taken as the minimum spatial distance for that traffic participant. When there exists any traffic participant i that satisfies: and / or If it is determined that a left-turning vehicle traveling along the initial left-turning trajectory will cause a conflict in the preset conflict area or insufficient safety distance, then it is considered that a yielding operation needs to be performed on the traffic participant, thereby determining that the trajectory secondary planning trigger condition is met.

[0148] Step 2.3: If the conditions for triggering secondary trajectory planning are met as determined in Step 2.2, based on the current traffic demand at the intersection and the priority traffic objects, and based on the current position and current speed direction of the left-turning vehicle along the initial left-turning trajectory, calculate the speed direction adjustment required to avoid oncoming straight-ahead vehicles or right-hand straight-ahead vehicles, and obtain the desired speed direction for secondary planning.

[0149] Specifically, at the trajectory secondary planning trigger time t, the latest state information of the left-turning vehicle at that time is read from the cache queue in step 2.1 to obtain the current position coordinates and current velocity direction of the left-turning vehicle in the first local coordinate system XOY, including the current position coordinates. Current driving speed and the angle between it and the positive Y-axis direction The vehicles identified in step 2.2 that have a potential conflict with left-turning vehicles and meet the following conditions will be considered. and / or The traffic participants under the conditions constitute a set To quantify the conflict severity for each traffic participant i, a time margin occupancy rate is defined. and distance margin occupancy rate for:

[0150] (11)

[0151] In the formula, The larger the value, the less time margin there is. A larger value indicates a less sufficient distance margin. Then, adjust according to the weighting coefficient. , ( and ≥0, preferably satisfying + =1) Constructing a conflict risk index:

[0152] + (12)

[0153] In the set The target with the highest risk index is selected as the priority passage target. :

[0154] (13)

[0155] If priority passage object If the vehicle is traveling straight in the opposite direction, then the current situation is determined to be a yielding scenario for oncoming straight traffic; if the vehicle with priority is... If the vehicle is coming from the right-hand straight lane, then the current situation is determined to be a right-hand straight lane yielding scenario.

[0156] Furthermore, based on priority access targets The sign for speed direction adjustment is determined by the type of scenario. In the scenario of yielding to oncoming straight traffic, to cause left-turning vehicles to deflect to the outside of the intersection, increase the turning radius, and prolong the time to reach the preset conflict area, the sign for the outward deflection direction is set as [symbol missing]. In scenarios where vehicles going straight on the right yield, to encourage left-turning vehicles to shorten their turning path towards the inside of the intersection, complete their left turns earlier, and reduce their obstruction of the merging area, the sign for the inside deflection direction is set as follows: .

[0157] Combined with priority access objects Risk index The magnitude of the velocity-direction adjustment of the left-turning vehicle at the current moment is obtained using a linear mapping method:

[0158] (14)

[0159] In the formula: A pre-calibrated scaling factor used to convert dimensionless hazard indicators into angular quantities. >0.

[0160] Considering constraints such as the vehicle's maximum permissible yaw rate, tire sideslip capability, and ride comfort (refer to the driving constraint parameters in step 1.4), the velocity direction adjustment is limited to obtain the final velocity direction adjustment:

[0161] (15)

[0162] In the formula: It is a saturation function; This is the preset maximum directional adjustment amplitude, used to ensure that the adjusted trajectory still meets vehicle dynamics constraints and comfort requirements. >0.

[0163] Based on this, the expected speed and direction of the left-turning vehicle at the moment of triggering can be obtained:

[0164] + (16)

[0165] Simultaneously, the angle parameter between the centerline direction of the exit lane and the desired speed direction of left-turning vehicles can be updated:

[0166] (17)

[0167] Finally, set the current position coordinates Current driving speed and the direction of the desired velocity This serves as the starting point for subsequent secondary planning. The original initial left-turn trajectory's departure target is maintained as the geometric reference, and only parameters are used... and the updated relative direction angle This reflects the need to yield to oncoming or right-hand vehicles. In step 3, using the aforementioned starting point state and updated direction parameters as input, a secondary planning is performed on the initial left-turn trajectory. The current position is considered as the starting point of the secondary planning trajectory, and a new left-turn trajectory that meets the yielding requirements is generated while ensuring safe distance and time interval.

[0168] Step 3: Establish a second local coordinate system with the current position of the left-turning vehicle in Step 2 as the origin and the expected speed direction of the left-turning vehicle at the trigger time as the Y-axis. Equivalently represent the current position and its driving direction as the virtual entrance lane of the quadratic planning trajectory, and equivalently represent the updated expected departure position and its corresponding driving direction as the virtual exit lane of the quadratic planning trajectory. Determine the position parameters based on the positional relationship between the virtual entrance lane and the virtual exit lane in the second local coordinate system.

[0169] The purpose of step 3 is to establish a new local coordinate system at the current position of the vehicle and to treat the remaining road segments as virtual entrance lanes and virtual exit lanes, thereby transforming the local quadratic programming problem within the intersection into a trajectory planning problem based on the geometric parameters of the virtual entrance and exit lanes. This facilitates the reuse of a unified trajectory model and simplifies the subsequent solution of the Bezier trajectory.

[0170] Specifically, see the instruction manual. Figure 4 , Figure 5 , Figure 6 , Figure 4 This diagram illustrates the coordinate system transformation of the position parameters of a vehicle turning left at an intersection to yield to a vehicle going straight on the right. Figure 5 This diagram illustrates the coordinate system transformation of the expected departure position of a vehicle turning left at an intersection, where the vehicle is yielding to a vehicle going straight on the right. Figure 6 A schematic diagram illustrating the trajectory parameter relationships for a left-turning vehicle at an intersection, where the vehicle must yield to a vehicle proceeding straight on the right. Step 3 includes:

[0171] Step 3.1: Obtain the current position coordinates of the left-turning vehicle in the first local coordinate system XOY at the trigger time. and current driving speed and direction The angle φ between the coordinate system and the positive Y-axis of the first local coordinate system is used as the starting point of the quadratic programming trajectory.

[0172] Step 3.2: Using the vehicle's current position in Step 3.1 as the origin O′, establish a second local coordinate system X′O′Y′, where the direction along the vehicle's current speed is the positive direction of the Y′ axis, and the direction perpendicular to the vehicle's current speed and pointing to the right of the vehicle is the positive direction of the X′ axis. The included angle φ is used to describe the rotational relationship between the first local coordinate system XOY and the second local coordinate system X′O′Y′.

[0173] Step 3.3: In the second local coordinate system X′O′Y′, the current position O′ of the left-turning vehicle at the time of the secondary planning trigger and its driving direction Y′ axis are equivalent to the virtual entrance lane of the secondary planning trajectory: O′ is the reference starting point of the virtual entrance lane, and the Y′ axis direction is the driving direction of the virtual entrance lane.

[0174] The virtual entrance lane width, lane center line, and other parameters are consistent with the actual lane where the left-turning vehicle is currently located.

[0175] Step 3.4: Based on the updated expected departure position obtained in Step 2.3, mark the coordinates of this expected departure position in the first local coordinate system XOY as ( , ), by coordinate translation and rotation, the point ( , Transform from the first local coordinate system XOY to the second local coordinate system X′O′Y′ to obtain the coordinates in X′O′Y′. , By combining the direction angle of the desired departure direction with the positive Y-axis direction of the first local coordinate system and the included angle φ, the direction angle of the desired departure direction with respect to the positive Y′ axis of the second local coordinate system can be obtained. , which serves as the driving direction angle for the virtual exit lane.

[0176] Where φ corresponds to the expected speed of the left-turning vehicle at the trigger moment in step 2.3. + Direction angle Formula (17) corresponds to step 2.3. .

[0177] Step 3.5: Based on the hazard index of the priority passage objects determined in Step 2.3 , final speed direction adjustment amount and the offset of the desired departure point relative to the current position determined in step 3.4. , ), calculate the vertical adjustment amount caused by yielding demand. .

[0178] When yielding to oncoming straight-ahead traffic, the departure position needs to be delayed, and the driving trajectory becomes the outer part of the initial trajectory and is longer. A positive value indicates that the original departure point will be moved backward in the "exit direction". For right-hand traffic yielding scenarios, the vehicle needs to leave the position earlier, and the driving trajectory will contract and shorten towards the inside of the initial trajectory. A negative value indicates that the original departure point will be moved forward in the "exit direction". Therefore, Δh is used to describe the longitudinal yield range, which is the longitudinal offset that must be compensated for when constructing the virtual exit lane.

[0179] Vertical adjustment amount for:

[0180] (18)

[0181] In the formula, For the scene symbols defined in step 2.3, yield to oncoming straight traffic. Give way to oncoming traffic on the right. . The vertical adjustment ratio coefficient is for offline calibration. >0.

[0182] Step 3.6: In the second local coordinate system X′O′Y′, the desired departure position and its driving direction in Step 3.4 are equivalent to virtual exit lanes. The position parameters are determined according to the relative positional relationship between the virtual entrance lane and the virtual exit lane: the displacement component along the X′ axis is used as the lateral distance parameter w′ of the virtual exit lane relative to the virtual entrance lane, the displacement component along the Y′ axis is used as the longitudinal distance parameter h′ of the virtual exit lane relative to the virtual entrance lane, and the angle between the driving direction of the virtual exit lane and the positive direction of the Y′ axis is used as the direction angle parameter α′. Thus, the parameter set (w′, h′, α′) describing the geometric relationship of the quadratic programming trajectory is obtained. The obtained (w′, h′, α′) is used as the input parameter for the quadratic programming trajectory generation step, and is used for the calculation of the distance parameter of the control point of the third-order Bézier curve in Step 4 and the construction of the quadratic programming trajectory.

[0183] For vehicles turning left to yield to vehicles going straight on the right, the expected displacement vector of the departure point relative to the virtual approach lane is:

[0184] (18)

[0185] In the formula, Corresponding to the X′O′Y′ coordinate system ; Corresponding to the X′O′Y′ coordinate system ; This refers to the longitudinal adjustment amount obtained in step 3.5; , The coordinates of the vehicle's current position in the first coordinate system at the trigger moment.

[0186] Lateral distance parameter between virtual exit lane and virtual entrance lane (In the direction of the X′ axis of the second coordinate system):

[0187] (19)

[0188] Longitudinal distance parameter between virtual exit lane and virtual entrance lane (Y′ axis direction in the second coordinate system) From the rotated y-component:

[0189] (20)

[0190] In formulas (19) and (20) The angle between the two coordinate systems .

[0191] For left-turning vehicles to yield to oncoming straight-ahead vehicles, the desired displacement vector of the departure point relative to the virtual approach lane is:

[0192] (twenty one)

[0193] Substituting these values ​​into the above formula will yield the corresponding horizontal and vertical distance parameters.

[0194] After translating to O′ and rotating to X′O′Y′, the virtual inlet and outlet geometric relationship (w′,h′,α′) is obtained.

[0195] Step 4: Based on the position parameters determined in Step 3 and the preset parameter mapping relationship, determine the control point distance parameters of the third-order Bézier curve, and construct the quadratic programming third-order Bézier trajectory of the left-turning vehicle in the second local coordinate system with the current position of the left-turning vehicle and the updated expected departure position as the endpoints.

[0196] The purpose of step 4 is to use the position parameters of the virtual entrance and exit lanes to quickly obtain the control points of the third-order Bézier curve through the preset parameter mapping relationship, so as to efficiently generate a secondary planning trajectory that adapts to the current environmental changes while satisfying vehicle dynamics constraints and trajectory smoothness, and realize the dynamic adjustment and continuous connection of the trajectory.

[0197] Specifically, see the instruction manual. Figure 6 Step 4 includes:

[0198] Step 4.1: Use the set of position parameters (w′, h′, α′) of the virtual exit lane relative to the virtual entrance lane and the set of vehicle driving constraint parameters determined in Step 3 as input for solving the Bézier control points in the quadratic programming.

[0199] Step 4.2: Substitute (w′, h′, α′) into the preset parameter mapping relationship or regression model to obtain the distance parameter of the first control point of the third-order Bézier curve in the quadratic programming. and synthetic distance parameters ( + ).

[0200] Specifically, the quadratic programming stage uses the following two sets of linear regression models, where, with The model with the dependent variable is:

[0201] h′(22)

[0202] The regression coefficients are given in Table 1:

[0203]

[0204] In Table 1, the variables include a constant and h, where the constant is the regression intercept term, corresponding to the value in model formula (22). The constant estimate here is 0.523. h is a variable (longitudinal distance parameter), corresponding to the regression term in model formula (22). h′, with an estimated coefficient of 0.407, indicates that for every 1m increase in h′, The average increase is 0.407 m. Column B contains the unstandardized regression coefficients, which are directly used in the regression equation. The constant B = 0.523, and the B for h is 0.407, meaning the final model is:

[0205] h′

[0206] The standard deviation represents the estimation uncertainty of the regression coefficient B; the smaller the value, the more stable the estimate. The constant standard deviation is 1.696, indicating a relatively large and unstable estimate. The standard deviation of h is 0.041, which is very small, indicating a stable estimate. β is the standardized regression coefficient, obtained by standardizing all variables, used to compare the importance of different independent variables. The β of h is 0.719, indicating that h is significant in relation to the regression coefficient. The influence of is relatively strong. The t column contains the significance test statistic for the coefficient. The p column is used to test whether the coefficient is significantly different from 0; p < 0.05 indicates significance, p < 0.01 indicates high significance, and for h, p < 0.01 indicates that h has a significant impact on . The effect is "extremely significant". The 95% confidence interval of B represents the interval in which the true regression coefficient B may fall at a 95% confidence level. The interval of h [0.326, 0.487] is all positive, further demonstrating that the positive effect of h is significant and reliable. R 2 This is an indicator of the model's explanatory power, representing... What percentage of the changes can be explained by h?

[0207] by( + The model with as the dependent variable is:

[0208] + (twenty three)

[0209] The regression coefficients are given in Table 2:

[0210]

[0211] In Table 2, the constant term corresponds to formula (23). |w|+h is the independent variable, representing the sum of the absolute value of the lateral distance and the longitudinal distance (the comprehensive distance characteristic of the virtual exit road relative to the virtual entrance road in quadratic programming), corresponding to formula (23). ,express For every additional 1 m, + The average increase was 0.709m. Other parameters have the same meaning as those in Table 1.

[0212] Step 4.3: Based on the vehicle's minimum permissible turning radius, maximum lateral acceleration, and comfort threshold, and Limit or correct the amplitude.

[0213] Step 4.3 Ensure that the quadratic programming trajectory meets the vehicle dynamics and comfort constraints.

[0214] Step 4.4: In the second local coordinate system X′O′Y′, take the current position O′ of the left-turning vehicle at the moment of triggering the curve as the starting point of the third-order Bézier curve. And the updated expected departure position E′ is used as the endpoint of the third-order Bézier curve. Travel along the virtual exit lane towards the destination. Inverse vector distance Determine the second control point The coordinates, along the Y′ axis of the virtual entrance lane from the starting point. Measure distance Determine the first control point The coordinates.

[0215] Step 4.5: Utilize , , , Four feature points are used to construct a third-order Bézier curve in the second local coordinate system, and this curve is used as the output of the quadratic planning traffic trajectory for left-turning vehicles.

[0216] Step 5: Control the left-turning vehicle according to the quadratic programming third-order Bezier trajectory generated in Step 4, so that the left-turning vehicle passes through the intersection along the quadratic programming third-order Bezier trajectory. If the quadratic programming trigger condition in Step 2 is not met, control the left-turning vehicle to pass through the intersection along the initial left-turning trajectory described in Step 1.

[0217] The purpose of step 5 is to apply the generated quadratic programming third-order Bezier trajectory to vehicle motion control, so that left-turning vehicles can smoothly pass through the intersection along the replanned trajectory while ensuring safe distance and driving stability; and to maintain driving along the initial trajectory when the quadratic programming is not triggered, thereby achieving a dynamic balance between safety and traffic efficiency.

[0218] Example

[0219] Taking a typical four-phase signalized intersection as an example, a left-turning vehicle (the target vehicle) is turning left from south to north into the westbound exit lane. A vehicle traveling straight in the same direction on its right is traveling straight from east to west. There is a risk of merging conflict between the target vehicle and the left-turning vehicle within the intersection (corresponding to the yielding situation for the right-of-way in the diagram). The target vehicle, road, and dynamic parameters are as follows:

[0220] The initial velocity of the vehicle turning left when entering the intersection is v0 = 2.68 m / s, and the initial longitudinal acceleration is a0 = 0.13 m / s².2 The maximum allowable acceleration for a left-turning vehicle during the initial starting phase is a. max =1.0 m / s 2 To ensure controllability and comfort during yielding, assume the maximum allowable deceleration during the yielding phase is a. min =0.5 m / s 2 .

[0221] Based on the geometric relationship between the approach and exit lanes of the intersection, initial position parameters (w, h, α) are extracted in the first local coordinate system XOY. Then, the distance parameters L1 and L2 of the third-order Bezier control points are obtained using offline regression mapping to construct the initial third-order Bezier trajectory. This trajectory satisfies the minimum turning radius and lateral acceleration constraints of vehicles without considering yield requirements.

[0222] As the left-turning vehicle travels along its initial trajectory, the system continuously senses the movement of the straight-going vehicle on its right, predicting the minimum time and space margins for both vehicles within the pre-defined merging conflict zone. When a condition is met... and / or At that time, it was determined that the vehicle must yield to the vehicle going straight on the right, triggering a secondary planning process.

[0223] The trigger time is denoted as t=4.1s. At this moment, the left-turning vehicle has approached the entrance to the conflict zone and has passed through the deceleration line to yield. A second local coordinate system X′O′Y′ is established with the vehicle's current position O′ as the origin. The direction of the vehicle's current desired speed is taken as the positive direction of the Y′ axis. The updated desired departure position is transformed to the second coordinate system to obtain the virtual exit lane parameters (w′,h′,α′). According to the trajectory data, the longitudinal distance deviation of the trigger point relative to the original exit lane centerline is Δh=2.78m. This deviation indicates that in order to allow the right-hand straight-ahead vehicle to pass through the merging point first, the left-turning vehicle needs to complete the turn in advance and shorten the longitudinal distance.

[0224] Based on the virtual position parameters in the second coordinate system, and by calling the regression models L1′=0.523+0.407h′ and L1′+L2′=-7.052+0.709(∣w′∣+h′), the distance parameters L1′ and L2′ of the quadratic programming control points are obtained. Control points P1′ and P2′ are then determined based on these parameters, using the current position P0′=O′ and the desired departure point P. d Using E' as the endpoint, a quadratic programming third-order Bezier trajectory is constructed. The characteristics of the quadratic trajectory include: "contraction" and shortening of the longitudinal distance in local areas, early avoidance of merging zones, and maintaining positional and directional continuity with the original trajectory at the trigger point. The controller selects the trajectory based on whether the trigger condition is met; if not triggered, it tracks the initial trajectory; once triggered, it switches to tracking the quadratic trajectory.

[0225] As per the instruction manual Figure 7 , Figure 8 , Figure 7 and Figure 8 This is a comparison chart of the actual trajectory and simulated trajectory of a left-turning vehicle, as well as the actual speed and simulated speed. Figure 7 In the diagram, the blue curve represents the simulated trajectory (generated and tracked after secondary planning using this method), and the orange curve represents the measured trajectory (the actual driver's yielding trajectory). The "1s–6s" markings on the curves indicate the vehicle's position at different times. In the 1s–3s phase, the two trajectories almost overlap, indicating that the vehicle continues to advance smoothly along its initial trajectory before fully entering the conflict zone. After approximately 4.1s, the secondary planning takes effect, and the simulated trajectory begins to contract towards the inside of the intersection, increasing its curvature and approaching the exit lane earlier, reducing the vehicle's occupation of the merging area before approaching the conflict point. The 4s–6s phase shows a significant difference; the simulated trajectory "contracts inward and merges into the exit lane earlier," while the actual trajectory is more outward. This may be due to drivers pre-steering, causing the vehicle to not fully enter the intersection along the centerline of the approach lane during the initial stage, resulting in a slight deviation. However, the overall trend remains consistent, indicating that this secondary planning model can effectively reproduce real yielding behavior.

[0226] Figure 8 In the diagram, blue represents the simulated speed curve, and orange represents the measured speed curve. The horizontal axis represents the time step (approximately 0.1 s), and the vertical axis represents speed in m / s. During the initial acceleration phase (0–approximately 3.5 s), both curves rise synchronously, indicating that the vehicle accelerates into the intersection according to the same longitudinal control objective, with the speed steadily increasing from 2.68 m / s to approximately 5–6 m / s. The difference in deceleration after yielding is as follows: the simulated curve begins to decelerate at approximately 4.2 s, while the measured curve shows significant deceleration only at approximately 5.3 s, indicating that the driver's yielding response is later than the system's prediction. The simulated deceleration is earlier and more significant, allowing the vehicle to effectively yield before the merging point; the measured speed decreases more gradually, reflecting that the driver completes the yielding through a combination of "slightly later deceleration + a more outer trajectory." After the simulated trajectory completes the yielding, the speed remains high and stable, while the measured speed slightly rebounds in the 60–80 time step, consistent with the driver's behavior of accelerating away again after confirming the conflict has been resolved.

[0227] As per the instruction manual Figure 9 , Figure 10 , Figure 9 and Figure 10 This is a comparison chart of the actual trajectory and simulated trajectory, and the actual speed and simulated speed of vehicles traveling straight. Among them, Figure 9 In the coordinate system, the horizontal axis represents the lateral position x (m) of the straight-moving vehicle in the first local coordinate system, and the vertical axis represents the longitudinal position y (m). Figure 9As can be seen, the simulated trajectory (blue) and the actual trajectory (orange) of the straight-moving vehicle are approximately horizontal straight paths. The two maintain a high degree of overlap throughout the entire journey, with only a slight longitudinal deviation at the initial stage, indicating that the y-value of the actual trajectory is slightly lower than that of the simulated trajectory. Subsequently, as the vehicle moves forward, the two trajectories gradually converge and tend to be consistent, indicating that the trajectory prediction / simulation model of the straight-moving vehicle can reflect its actual driving path well. Figure 10 In the diagram, the horizontal axis represents the time step (approximately 0.1 s), and the vertical axis represents speed (m / s). It can be seen that the speed of vehicles traveling straight through the intersection remains relatively stable before and after entering the intersection. The simulated speed curve decreases slightly in the first half and gradually recovers after about 40 time steps; the actual speed curve shows a slow decrease followed by stabilization. The trends of both are consistent, and the speed difference is small during peak traffic periods, indicating that the simulated speed can reasonably depict the real speed change pattern of vehicles traveling straight through the intersection, characterized by a slight decrease followed by recovery. Figure 8 and Figure 9 Together, they show that the simulated trajectory and speed of straight-going vehicles are in good agreement with the measured results in the intersection scenario, which can provide reliable motion input of oncoming / lateral straight-going vehicles for conflict prediction and yielding decision in the secondary planning of left-turning vehicles.

[0228] In this embodiment, when the right-hand traffic has priority, the method can trigger secondary planning in 4.1 seconds. By shortening the longitudinal distance by Δh=2.78m and updating the virtual entrance and exit lane parameters, it generates an inward-facing third-order Bezier quadratic trajectory and tracks and controls it in real time, enabling left-turning vehicles to complete the turn in advance and reducing their occupation of the merging area. Moreover, the generated trajectory and speed change trends are consistent with the actual driving behavior, verifying the effectiveness and feasibility of the method in the right-hand traffic yielding scenario at intersections.

[0229] This invention also provides a secondary planning system for the trajectory of left-turning vehicles at intersections, the system comprising:

[0230] The initial trajectory generation module is used to establish a first local coordinate system in the intersection area based on the geometric positional relationship between the intersection entrance lane and the exit lane and the driving constraints of left-turning vehicles, and to generate the initial left-turning trajectory of left-turning vehicles passing through the intersection in the first local coordinate system.

[0231] The trigger determination and departure update module is used to acquire the status information of the left-turning vehicle and the intersection environment information in real time during the left-turning vehicle's journey along the initial left-turning traffic trajectory. Based on the status information and environment information, it determines whether the trajectory secondary planning trigger condition is met. When the trigger condition is met, it acquires the current position and speed direction of the left-turning vehicle and determines the updated expected departure position based on the current traffic demand at the intersection.

[0232] The second coordinate system and position parameter determination module is used to establish a second local coordinate system with the current position of the left-turning vehicle obtained by the trigger judgment and departure update module as the origin and the expected speed direction of the left-turning vehicle at the trigger time as the Y-axis. The current position and its driving direction are equivalent to the virtual entrance lane of the quadratic planning trajectory, and the updated expected departure position and its corresponding driving direction are equivalent to the virtual exit lane of the quadratic planning trajectory. The position parameters of the quadratic planning are determined according to the relative positional relationship between the virtual entrance lane and the virtual exit lane in the second local coordinate system.

[0233] The quadratic programming Bézier curve trajectory generation module is used to determine the control point distance parameters of the quadratic programming third-order Bézier curve based on the position parameters determined by the second coordinate system and the position parameter determination module, as well as the preset parameter mapping relationship. It then constructs the quadratic programming third-order Bézier curve trajectory of the left-turning vehicle in the second local coordinate system with the current position of the left-turning vehicle and the updated expected departure position as endpoints.

[0234] The trajectory tracking control module is used to control the driving of left-turning vehicles according to the quadratic programming Bézier trajectory generated by the quadratic programming Bézier trajectory generation module, so that the left-turning vehicles pass through the intersection along the quadratic programming Bézier curve trajectory, and control the left-turning vehicles to pass through the intersection along the initial left-turning traffic trajectory if the trajectory quadratic programming trigger condition is not met.

[0235] The above descriptions are merely embodiments of this application, and common knowledge regarding specific structures and characteristics in the solutions is not described in detail here. It will be apparent to those skilled in the art that this application is not limited to the details of the above exemplary embodiments, and that this application can be implemented in other specific forms without departing from the spirit or essential characteristics of this application. Therefore, the embodiments should be considered exemplary and non-limiting in all respects, and the scope of this application is defined by the appended claims rather than the foregoing description. Therefore, it is intended that all variations falling within the meaning and scope of equivalents of the claims be included within this application. No reference numerals in the claims should be construed as limiting the scope of the claims.

Claims

1. A method for quadratic planning of the trajectory of left-turning vehicles at an intersection, characterized in that, The method includes: Step 1: Based on the geometric positional relationship between the intersection's approach lanes and exit lanes and the driving constraints of left-turning vehicles, establish a first local coordinate system in the intersection area, and generate the initial left-turning trajectory of left-turning vehicles passing through the intersection under the first local coordinate system. Step 2: During the process of the left-turning vehicle traveling along the initial left-turning trajectory, the status information of the left-turning vehicle and the intersection environment information are obtained in real time. When it is determined that the trajectory secondary planning trigger condition is met based on the status information and environment information, the current position and speed direction of the left-turning vehicle are obtained, and the updated expected departure position is determined based on the current traffic demand of the intersection. Step 3: Establish a second local coordinate system with the current position of the left-turning vehicle in Step 2 as the origin and the expected speed direction of the left-turning vehicle at the trigger time as the Y-axis. Equivalently represent the current position and its driving direction as the virtual entrance lane of the quadratic planning trajectory, and equivalently represent the updated expected departure position and its corresponding driving direction as the virtual exit lane of the quadratic planning trajectory. Determine the position parameters based on the positional relationship between the virtual entrance lane and the virtual exit lane in the second local coordinate system. Step 4: Based on the position parameters determined in Step 3 and the preset parameter mapping relationship, determine the control point distance parameters of the third-order Bézier curve, and construct the quadratic programming third-order Bézier trajectory of the left-turning vehicle in the second local coordinate system with the current position of the left-turning vehicle and the updated expected departure position as the endpoints. Step 5: Control the left-turning vehicle according to the quadratic programming third-order Bezier trajectory generated in Step 4, so that the left-turning vehicle passes through the intersection along the quadratic programming third-order Bezier trajectory. If the quadratic programming trigger condition in Step 2 is not met, control the left-turning vehicle to pass through the intersection along the initial left-turning trajectory described in Step 1.

2. The method for secondary planning of the trajectory of left-turning vehicles at intersections according to claim 1, characterized in that, Step 1 includes: Step 1.1: Obtain the geometric information of the intersection's approach and exit lanes; Step 1.2: Taking the intersection approach lane as the reference, set the midpoint of the parking line of the approach lane as the origin O of the first local coordinate system XOY, set the extension line of the parking line as the X-axis, and set the direction perpendicular to the parking line and pointing into the intersection as the Y-axis. Establish the first local coordinate system for left turn trajectory planning in the intersection area. Step 1.3: In the first local coordinate system XOY, based on the relative position and direction of the centerline of the exit lane and the centerline of the inlet lane, determine the position parameters of the exit lane relative to the inlet lane. Among them, the horizontal distance along the X-axis represents the lateral distance parameter w, the vertical distance along the Y-axis represents the longitudinal distance parameter h, and the angle between the direction of the centerline of the exit lane and the positive direction of the Y-axis represents the direction angle parameter α. Step 1.4: Based on the vehicle model parameters and driving conditions of the vehicle turning left, obtain the driving constraints that affect the trajectory planning; Step 1.5: Based on the position parameters w, h, and α obtained in Step 1.3, and the driving constraints in Step 1.4, the control point distance parameters of the third-order Bézier curve are obtained through a pre-established parameter mapping relationship or regression model. and ,Depend on , Determine the coordinates of each control point of the third-order Bézier curve in the first local coordinate system XOY; Step 1.6: In the first local coordinate system XOY, take the reference starting point of the left-turning vehicle at the stop line of the entrance lane as the starting point of the third-order Bézier curve, and take the reference ending point of the center line of the exit lane at the exit position of the intersection as the ending point of the third-order Bézier curve. Combine the control point coordinates determined in Step 1.5 to construct the third-order Bézier curve, and use the third-order Bézier curve as the initial left-turning trajectory of the left-turning vehicle through the intersection.

3. The method for secondary planning of the trajectory of left-turning vehicles at intersections according to claim 1, characterized in that, Step 2 includes: Step 2.1: While the left-turning vehicle is traveling along the initial left-turning trajectory, acquire the status information of the left-turning vehicle and the intersection environment information in real time; Step 2.2: Based on the status information of the left-turning vehicle, the intersection environment information, and the initial left-turning trajectory, predict the spatiotemporal relationship between the left-turning vehicle and other traffic participants when the left-turning vehicle passes through the preset conflict area along the initial left-turning trajectory. When it is determined that the left-turning vehicle will cause a conflict in the preset conflict area or insufficient safety distance under the preset safety distance and / or preset safety time interval, determine that the trajectory secondary planning trigger condition is met. Step 2.3: If the conditions for triggering secondary trajectory planning are met as determined in Step 2.2, based on the current traffic demand at the intersection and the priority traffic objects, and based on the current position and current speed direction of the left-turning vehicle along the initial left-turning trajectory, calculate the speed direction adjustment required to avoid oncoming straight-ahead vehicles or right-hand straight-ahead vehicles, and obtain the desired speed direction for secondary planning.

4. The method for secondary planning of the trajectory of left-turning vehicles at intersections according to claim 3, characterized in that, Step 3 includes: Step 3.1: Obtain the current position coordinates of the left-turning vehicle in the first local coordinate system XOY at the trigger time. and current driving speed and direction The angle φ between the coordinate system and the positive Y-axis of the first local coordinate system is used as the starting point of the quadratic programming trajectory. Step 3.2: Using the vehicle's current position in Step 3.1 as the origin O′, establish a second local coordinate system X′O′Y′, where the direction along the vehicle's current speed is the positive direction of the Y′ axis, and the direction perpendicular to the vehicle's current speed and pointing to the right of the vehicle is the positive direction of the X′ axis. The included angle φ is used to describe the rotational relationship between the first local coordinate system XOY and the second local coordinate system X′O′Y′. Step 3.3: In the second local coordinate system X′O′Y′, the current position O′ of the left-turning vehicle at the time of the secondary planning trigger and its driving direction Y′ axis are equivalent to the virtual entrance lane of the secondary planning trajectory: O′ is the reference starting point of the virtual entrance lane, and the Y′ axis direction is the driving direction of the virtual entrance lane; Step 3.4: Based on the updated expected departure position obtained in Step 2.3, mark the coordinates of this expected departure position in the first local coordinate system XOY as ( , ), by coordinate translation and rotation, the point ( , Transform from the first local coordinate system XOY to the second local coordinate system X′O′Y′ to obtain the coordinates in X′O′Y′. , By combining the direction angle of the desired departure direction with the positive Y-axis direction of the first local coordinate system and the included angle φ, the direction angle of the desired departure direction with respect to the positive Y′ axis of the second local coordinate system can be obtained. , serving as the driving direction angle of the virtual exit lane; Step 3.5: Based on the hazard index of the priority passage objects determined in Step 2.3 , final speed direction adjustment amount and the offset of the desired departure point relative to the current position determined in step 3.

4. , ), calculate the vertical adjustment amount caused by yielding demand. ; Step 3.6: In the second local coordinate system X′O′Y′, the desired departure position and its driving direction in Step 3.4 are equivalent to virtual exit lanes. The position parameters are determined according to the relative positional relationship between the virtual entrance lane and the virtual exit lane: the displacement component along the X′ axis is used as the lateral distance parameter w′ of the virtual exit lane relative to the virtual entrance lane, the displacement component along the Y′ axis is used as the longitudinal distance parameter h′ of the virtual exit lane relative to the virtual entrance lane, and the angle between the driving direction of the virtual exit lane and the positive direction of the Y′ axis is used as the direction angle parameter α′. Thus, the parameter set (w′, h′, α′) describing the geometric relationship of the quadratic programming trajectory is obtained. The obtained (w′, h′, α′) is used as the input parameter for the quadratic programming trajectory generation step, and is used for the calculation of the distance parameter of the control point of the third-order Bézier curve in Step 4 and the construction of the quadratic programming trajectory.

5. The method for secondary planning of the trajectory of left-turning vehicles at intersections according to claim 1, characterized in that, Step 4 includes: Step 4.1: Use the set of position parameters (w′, h′, α′) of the virtual exit lane relative to the virtual entrance lane and the set of vehicle driving constraint parameters determined in Step 3 as input for solving the Bézier control points in the quadratic programming. Step 4.2: Substitute (w′, h′, α′) into the preset parameter mapping relationship or regression model to obtain the distance parameter of the first control point of the third-order Bézier curve in the quadratic programming. and synthetic distance parameters ( + ); Step 4.3: Based on the vehicle's minimum permissible turning radius, maximum lateral acceleration, and comfort threshold, and Limit or correct the amplitude; Step 4.4: In the second local coordinate system X′O′Y′, take the current position O′ of the left-turning vehicle at the moment of triggering the curve as the starting point of the third-order Bézier curve. And the updated expected departure position E′ is used as the endpoint of the third-order Bézier curve. Travel along the virtual exit lane towards the destination. Inverse vector distance Determine the second control point The coordinates, along the Y′ axis of the virtual entrance lane from the starting point. Measure distance Determine the first control point The coordinates; Step 4.5: Utilize , , , Four feature points are used to construct a third-order Bézier curve in the second local coordinate system, and this curve is used as the output of the quadratic planning traffic trajectory for left-turning vehicles.

6. The method for secondary planning of the trajectory of left-turning vehicles at intersections according to claim 2, characterized in that, In step 1.1, the geometric information of the intersection approach lanes and exit lanes includes the location of the stop line of the approach lane, the direction of the center lines of the approach lanes and exit lanes, and the range of the passable area inside the intersection.

7. The method for secondary planning of the trajectory of left-turning vehicles at intersections according to claim 3, characterized in that, In step 2.2, the trigger condition for trajectory quadratic programming is: there exists any traffic participant i that satisfies... and / or ,in, The shortest time interval between a left-turning vehicle and each traffic participant within a predefined conflict zone. For the safety time interval threshold associated with left-turning vehicles, Let be the minimum spatial distance of a vehicle to traffic participant i at the predicted location at time t. The longitudinal safety distance threshold associated with left-turning vehicles; In step 2.3, the speed and direction adjustment required for a left-turning vehicle to avoid oncoming or right-hand traffic. for: ； In the formula: The symbol indicating the direction of deflection for vehicles turning left; It is a saturation function; This represents the magnitude of the speed and direction adjustment for left-turning vehicles at the current moment. Adjust the amplitude in the preset maximum direction.

8. The method for secondary planning of the trajectory of left-turning vehicles at intersections according to claim 7, characterized in that, In step 3.4, the desired speed and direction of the left-turning vehicle at the moment of triggering. for: + ； In the formula: The current speed of the vehicle turning left And the angle between its direction of travel and the positive direction of the Y-axis; The amount of speed and direction adjustment required for left-turning vehicles to avoid oncoming or right-hand vehicles going straight. The angle parameter between the centerline direction of the exit lane and the desired speed direction of left-turning vehicles for: ； In the formula: The angle between the direction of the centerline of the exit channel in the first local coordinate system XOY and the positive direction of the Y-axis; The desired speed and direction of the left-turning vehicle at the moment of triggering; In step 3.5, the longitudinal adjustment amount for left-turning vehicles. for: ； In the formula, The symbol indicating the direction of deflection for vehicles turning left; For offline calibration, the longitudinal adjustment ratio coefficient; The risk level index for priority passage objects.

9. The method for secondary planning of the trajectory of left-turning vehicles at intersections according to claim 8, characterized in that, In step 3.6, regarding the left-turning vehicle yielding to the right-hand through vehicle, the expected displacement vector of the departure point relative to the virtual approach lane is: ； In the formula: Corresponding to the X′O′Y′ coordinate system ; Corresponding to the X′O′Y′ coordinate system ; For left-turning vehicles, the longitudinal adjustment amount is used. , The coordinates of the vehicle's current position in the first coordinate system at the moment of triggering; For left-turning vehicles to yield to oncoming straight-ahead vehicles, the desired displacement vector of the departure point relative to the virtual approach lane is: ； Lateral distance parameter between virtual exit lane and virtual entrance lane for: ； Longitudinal distance parameter between virtual exit lane and virtual entrance lane for: 。 10. A quadratic planning system for the trajectory of left-turning vehicles at an intersection, characterized in that, The system includes: The initial trajectory generation module is used to establish a first local coordinate system in the intersection area based on the geometric positional relationship between the intersection entrance lane and the exit lane and the driving constraints of left-turning vehicles, and to generate the initial left-turning trajectory of left-turning vehicles passing through the intersection in the first local coordinate system. The trigger determination and departure update module is used to acquire the status information of the left-turning vehicle and the intersection environment information in real time during the left-turning vehicle's journey along the initial left-turning traffic trajectory. Based on the status information and environment information, it determines whether the trajectory secondary planning trigger condition is met. When the trigger condition is met, it acquires the current position and speed direction of the left-turning vehicle and determines the updated expected departure position based on the current traffic demand of the intersection. The second coordinate system and position parameter determination module is used to establish a second local coordinate system with the current position of the left-turning vehicle obtained by the trigger judgment and departure update module as the origin and the expected speed direction of the left-turning vehicle at the trigger time as the Y-axis. The current position and its driving direction are equivalent to the virtual entrance lane of the quadratic planning trajectory, and the updated expected departure position and its corresponding driving direction are equivalent to the virtual exit lane of the quadratic planning trajectory. The position parameters of the quadratic planning are determined according to the relative positional relationship between the virtual entrance lane and the virtual exit lane under the second local coordinate system. The quadratic programming Bézier curve trajectory generation module is used to determine the control point distance parameters of the quadratic programming third-order Bézier curve based on the position parameters determined by the second coordinate system and the position parameter determination module, as well as the preset parameter mapping relationship. It also constructs the quadratic programming third-order Bézier curve trajectory of the left-turning vehicle in the second local coordinate system with the current position of the left-turning vehicle and the updated expected departure position as the endpoints. The trajectory tracking control module is used to control the driving of left-turning vehicles according to the quadratic programming Bézier trajectory generated by the quadratic programming Bézier trajectory generation module, so that the left-turning vehicles pass through the intersection along the quadratic programming Bézier curve trajectory, and control the left-turning vehicles to pass through the intersection along the initial left-turning traffic trajectory if the trajectory quadratic programming trigger condition is not met.

Images