Urban road trajectory planning and control method and system

Abstract

The invention belongs to the technical field of trajectory planning, and discloses an urban road trajectory planning and control method and system, and the method comprises the steps: generating an obstacle set and a Frenet projection of a road physical boundary through employing a spatial clustering algorithm; utilizing an extreme value method and a self-adaptive smoothing algorithm to dynamically construct a feasible trajectory region trajectory, and synchronously performing interval constraint expansion on a future prediction trajectory of the dynamic obstacle; constructing a path optimization objective function with interval constraints, and solving an optimal path variable in real time by using a numerical optimization method; an expected speed and acceleration sequence of each sampling point is generated based on a space-time domain dynamic planning method, and a longitudinal motion curve is optimized in real time by using an objective function; and inputting the space path and the longitudinal speed sequence into an MPC system, and calculating an optimal front wheel steering angle and acceleration control instruction to realize vehicle trajectory tracking. The method can sense errors and environment changes in a self-adaptive mode, the response speed and robustness of dynamic traffic flow are improved, and multi-target collaborative optimization is achieved.

Description

TECHNICAL FIELD

[0001] The application belongs to the technical field of trajectory planning, and particularly relates to a city road trajectory planning and control method and system. BACKGROUND

[0002] Automatic driving technology has become a core research direction in the field of intelligent transportation and intelligent networked vehicles. With the acceleration of urbanization, the structure of urban roads is becoming increasingly complex, characterized by multiple lanes, frequent temporary obstacles, diverse behaviors of traffic participants, and an increasing number of unexpected events. In such scenarios, an autonomous vehicle not only needs to accurately understand the road structure and obstacle distribution, but also needs to have real-time and efficient trajectory planning and dynamic obstacle avoidance capabilities. Traditional trajectory planning methods, such as A* and RRT-based graph search algorithms, can provide global path optimization, but they face challenges such as poor real-time performance and non-smooth paths. Algorithms based on interpolation curves, such as Bezier or B-spline, improve path continuity, but they struggle to deal with dense and dynamic obstacles and vehicle dynamics constraints. In recent years, the Frenet coordinate system has been widely used in path planning, as it can decompose the vehicle's motion in the global coordinate system into longitudinal motion along the reference line and lateral deviation, significantly simplifying the problem dimension and improving computational efficiency. However, current mainstream Frenet path planning algorithms still have issues such as poor adaptability to complex obstacles, delayed trajectory interval updates, and insufficient robustness to perception noise and dynamic prediction, especially in urban structural roads.

[0003] Current automatic driving trajectory planning mainly uses methods based on graph search, interpolation curves, numerical optimization, and sampling. Typical schemes include A* and RRT global search algorithms, which discretize the vehicle's operating environment into a grid and use heuristic search to obtain a feasible path from the starting point to the end point. There are also methods that use Bezier curves and B-spline interpolation to achieve path continuity and smooth transitions. In addition, some systems combine vehicle dynamics constraints and use model predictive control or dynamic window optimization methods to improve the feasibility and safety of the trajectory. In recent years, the Frenet coordinate system has been widely introduced into trajectory planning, which maps the global path to a reference line and projects the vehicle and obstacles into the Frenet space, achieving decomposed optimization of the path and speed, improving computational efficiency and adaptability. Some research further proposes local path planning methods based on constraint regions or feasible intervals, aiming to improve planning results in scenarios with dense and dynamic obstacles.

[0004] The existing trajectory planning method has the following specific deficiencies in the urban structural road environment: when the obstacles are diverse and densely distributed, the traditional path scheme based on static preset or global search is difficult to realize efficient avoidance of dynamic obstacles and real-time update of the trajectory, resulting in that the vehicle is not timely in responding to sudden traffic incidents or temporary roadblock scenes, the path is not smooth, and the safety and comfort are affected. Although the existing Frenet coordinate system related method improves the efficiency of path planning, it is still rough in the aspects of feasible trajectory interval generation, obstacle classification and dynamic adjustment mechanism, and often relies on prior parameters, which is difficult to adapt to complex road structure and high dynamic traffic flow environment; and lacks multi-objective collaborative optimization of vehicle dynamics constraints, trajectory smoothness and safety distance, resulting in obvious deficiencies of the generated trajectory in physical feasibility, dynamic response and system robustness, which limits the practicality and popularization of autonomous vehicles in complex urban roads.

[0005] Through the above analysis, the problems and defects of the prior art are:

[0006] (1) Real-time and self-adaptation are insufficient: the trajectory planning based on traditional A*, RRT and other graph search algorithms has poor real-time performance, the path is not smooth, and the ability to respond to sudden obstacles is limited. Although the interpolation curve algorithm can improve the continuity of the path, it performs poorly in dense and dynamic obstacle environments and in the face of complex vehicle dynamics constraints, making it difficult to ensure the actual running safety and efficiency in urban road scenarios.

[0007] (2) Static corridor boundary and update lag: the existing Frenet path planning algorithm often uses a static or preset trajectory corridor interval, which is difficult to reflect the temporary changes of the road and the distribution of dynamic obstacles in a timely manner, resulting in delayed trajectory interval update and poor adaptability to complex obstacles, especially in special scenarios such as high-speed ramps, crossroads or roadside parking caused temporary bottlenecks, it cannot accurately give the real-time safe and feasible trajectory region.

[0008] (3) Weak robustness to perception noise and dynamic obstacle prediction: when the perception error is large and the dynamic obstacle behavior is uncertain, the mainstream method is difficult to maintain the trajectory feasibility and system stability, and is prone to phenomena such as corridor boundary crossing and trajectory planning failure, which cannot effectively respond to data fluctuations and dynamic changes in real traffic scenarios.

[0009] (4) Lack of coordination between vehicle dynamics and multi-objective optimization: most of the existing Frenet coordinate system related methods only use spatial geometry as a constraint, and do not effectively introduce physical constraints such as vehicle nonlinear dynamics, continuous multi-steering behavior, curvature constraints and actual driving safety distance into the optimization framework, resulting in a lack of physical feasibility and comfort of the generated trajectory, and insufficient response to the actual needs of the vehicle control system.

[0010] (5) Path-speed integrated optimization exists limit bottleneck: the traditional method adopts path and speed integrated optimization, it is difficult to separate and close-loop adaptively adjust for the complex road sections of city (such as temporary narrow lane, intersection area), lead to in continuous multi-turn or sudden obstacle scene, trajectory peak, speed jitter and frequent stagnation and re-planning problems, seriously affect the smooth tracking and high-frequency dynamic response of automatic driving vehicle.

[0011] (6) Lack of full-process adaptive multi-objective robust optimization: the existing scheme usually focuses on a local link (such as obstacle avoidance or path smoothing), and cannot realize the full-process multi-objective coordination and high-robustness closed-loop adaptive optimization from environment perception-corridor generation-path optimization-speed planning-trajectory tracking, limiting the overall engineering practicability and generalizability of the system. SUMMARY

[0012] In order to overcome the problems in the related art, the present application discloses a city road trajectory planning and control method and system, especially a city road trajectory planning and control method and system based on real-time constraint trajectory interval and Frenet coordinate system, the technical solution is as follows:

[0013] The present application is implemented in a city road trajectory planning and control method, comprising the following steps:

[0014] S1, environment perception and modeling; based on multi-source perception equipment, data fusion is carried out on the city road environment; space clustering algorithm is used to unify road elements and obstacles into structured objects and map them to Frenet coordinate system, to generate Frenet projection of obstacle set and road physical boundary;

[0015] S2, path optimization and high-order curve fitting; in multiple scenarios, adaptive feasible trajectory region generation strategy is adopted, extreme-adaptive smoothing hybrid algorithm is used to construct Frenet region boundary in single traversal, for complex scenarios of high-speed ramp, crossroads and narrow bottleneck caused by roadside parking, dynamically and accurately update the safe and feasible trajectory region, and update the upper and lower boundaries according to the sampling points in real time, adaptively expand to the dynamic obstacle prediction trajectory, overcome the hysteresis problem of traditional static region method, provide reliable initial constraint for subsequent optimization, significantly improve the robustness and timeliness of the system;

[0016] S3, Path Optimization and High-Order Curve Fitting: Based on spatial domain improvement of vehicle kinematics model, coupled with multiple steering constraints, the traditional nonlinear vehicle model is transformed from the time domain to the spatial domain, and a smooth steering sequence is continuously generated under the curvature-velocity relaxation mechanism, constructing a multi-objective collaborative convex optimization framework. Collision constraints are transformed into region boundaries, and lateral offset, curvature penalty, and dynamic risk Gaussian terms are unified into an approximate convex quadratic objective function, optimizing path variables in real time and improving the comfort and safety margin of vehicles during multi-segment steering in narrow roads;

[0017] S4, longitudinal velocity and acceleration planning; adopts a path-velocity separation strategy and couples it with a closed loop of model predictive control (MPC), and generates the expected velocity and acceleration sequence for each sampling point based on the space-time domain dynamic programming method, so as to achieve expected velocity tracking, acceleration smoothing and obstacle safety constraints, solve the curvature peak and velocity jitter problem in continuous multi-turn narrow road scenarios, and avoid frequent replanning and stagnation in temporary narrow roads.

[0018] S5, Trajectory Tracking and Vehicle Control Execution: Input the spatial path and longitudinal velocity sequence into the vehicle model predictive control system, calculate the optimal front wheel steering angle and acceleration control commands to achieve vehicle trajectory tracking.

[0019] In step S1, road structures, lane lines, traffic signs, static obstacles, and dynamic obstacles are detected and located using multi-source sensing devices. A multi-object fusion spatial clustering algorithm and density estimation algorithm are then used to uniformly represent the original sensed point cloud and image information as an obstacle set O in the global coordinate system, denoted as O = {o}. i |o i =(x i ,y i ,l i ,w i ,v xi ,v yi )}, where o i Let x be the attribute tuple of obstacle O. i ,y i Let l be the global spatial position of obstacle i, and l be the global spatial position of obstacle i. i ,w i v represents the length and width of the obstacle. xi ,v yi This represents the velocity components of the obstacle in the global coordinate system.

[0020] The obstacle set and road structure information are mapped to the Frenet coordinate system through projection transformation; the Frenet coordinate system uses a global reference line as a reference, defining a longitudinal s-axis and a transverse l-axis, and defining the coordinate system for each obstacle point (x...). i ,y i Perform coordinate transformation The reference line is represented in the global coordinate system asref = {(x ref (s),y ref (s))}, the longitudinal projection s i is obtained by minimizing the Euclidean distance;

[0021]

[0022] lateral offset l i is calculated by the normal vector of the obstacle point and the projection point;

[0023] l i = [(x i ,y i )-(x ref (s i ),y ref (s i ))]·n(s i )

[0024] wherein n(s i ) is the unit normal vector of the reference line at s i , which is obtained by the normal transformation of the reference line tangent vector t(s i ); based on the multi-model state estimation algorithm including Kalman filtering, the future time domain trajectory of the dynamic obstacle is predicted to generate the spatiotemporal prediction set wherein T is the planning prediction time domain; the trajectory prediction result of the dynamic obstacle is projected in each time by Frenet coordinates.

[0025] Further, the multi-source perception device includes a laser radar, a camera, a millimeter wave radar and a high-precision map, the static obstacle includes a parked vehicle, a guardrail and a road facility, the dynamic obstacle includes a pedestrian, a non-motor vehicle and other vehicles; the global reference line is a road center line or a preset path, the s axis is along the arc length direction of the reference line, and the l axis is perpendicular to the reference line direction.

[0026] Further, the feasible trajectory corridor generation strategy can adaptively work in multiple scenes such as high-speed ramps, crossroads and roadside parking narrow lanes, and in step S2, the feasible trajectory interval dynamic generation comprises: discretizing the longitudinal path into a plurality of sampling points according to the current position of the vehicle and the planning interval; according to the obstacle projection and the road boundary information at each sampling point, the corresponding lateral feasible interval is dynamically calculated by using the extremum method and the adaptive smoothing algorithm, the feasible trajectory region covering the entire planning section is constructed, and the future predicted trajectory of the dynamic obstacle is simultaneously subjected to interval constraint expansion.

[0027] In step S2, the feasible trajectory interval dynamic generation specifically comprises:

[0028] According to the reference path of the vehicle in the planning period, the longitudinal arc length interval [s0, s N ] is discretized into an equidistant sequence of sampling points Each sampling point corresponds to a set of lateral interval boundary parameters.

[0029] At each sampling point s k , the system retrieves all obstacle and road boundary information at this longitudinal position, and generates the lower and upper boundaries based on the obstacle coordinates (s i , l i ) projected by the perception module using the maximum-minimum extreme value method. The specific calculation process is as follows:

[0030] For all obstacles o i in the set O, if the longitudinal projection s i satisfies |s i -s k | < ∈, where ∈ is the distance tolerance, then the lateral position l i can be regarded as a constraint point for the current sampling point s k ; based on the physical boundary of the road The maximum value of all lower constraint points is taken as the lower bound of the current lower feasible trajectory interval, and the minimum value of all upper constraint points is taken as the upper bound of the current upper feasible trajectory interval.

[0031]

[0032] wherein represents the set of obstacle indices located to the left or below the vehicle near the s k position, is the set of obstacle indices on the right or above, l lb (s k ) and l ub (s k ) are the original lower / upper boundaries obtained by the extreme value method.

[0033] An adaptive interval smoothing algorithm is introduced, and a window W (length M sampling points) and an adaptive weight are defined.

[0034] ω k = exp (-|l b (s k )-l b (s k-1 )| / σ)

[0035] wherein l b (s k ) is the constraint point of the edge sampling point s k of the feasible trajectory region, ω k and σ are the adaptive smoothing weight and sensitivity parameters.

[0036] The preliminary generated boundary sequence {l lb (s k )} is applied with sliding mean filtering; ub (s k );

[0037]

[0038] wherein, and the smoothed and expanded final feasible interval upper and lower boundaries, and M is the length of the smoothing window;

[0039] Similarly, the dynamic obstacle predicted trajectory is mapped and participates in the calculation of the interval boundary. If the obstacle has a speed component, the future projection point in the planning time domain is predicted, and the most unfavorable value of the lateral position at all times is taken to update the interval boundary;

[0040] For special urban road structures, the parameters of the physical boundary are adjusted in real time according to high-precision maps or online perception, realizing adaptive matching with the actual road environment, and the feasible trajectory interval of each sampling point is output as an interval sequence , wherein the special urban road structures include variable lanes, road narrowing or merging, and temporary closed areas.

[0041] In step S3, based on the feasible lateral interval of each sampling point in the Frenet coordinate system , an optimal path curve is generated by combining vehicle dynamics constraints, trajectory continuity, smoothness, and obstacle risk penalty terms; the path of the entire planning interval is modeled as a set of discrete lateral offset variables k , wherein each l k represents the lateral offset at the longitudinal sampling point s k , satisfying the spatial interval constraint A high-order polynomial or spline curve is used to fit the path, and each path is represented on the interval [s k+1 , s k ] as:

[0042] l(s)=a0+a1(s-s k ) 2 +a2(s-s k ) 3 +a3(s-s k ) 4 +a4(s-s k ) 5

[0043] Where a0,…,a5 are polynomial coefficients, and a globally continuous differentiable curve is constructed by concatenating the values, derivatives and second derivatives of the path at the sampling points.

[0044] The path optimization objective function is expressed in terms of polynomial coefficients or discrete point set {l k Let} be the decision variable, represented as:

[0045]

[0046] Where, Δs=s k+1 -s k l is the sampling interval. ref (s k () is the reference line at s k The horizontal position at point R, where w1 and w2 are weighting coefficients, R obs (l k ,s k The first term represents the obstacle risk penalty term, which applies weighted suppression to paths approaching obstacles; the second term introduces a risk factor for perceived obstacles and employs a Gaussian penalty function.

[0047]

[0048] in, For in s k The set of nearby obstacles, λ i Let σ be the penalty weight for obstacle i. i This indicates the uncertainty of the obstacle boundary.

[0049] In step S4, the longitudinal velocity and acceleration planning includes:

[0050] Based on the spatially optimal path curve determined in step S3, and considering vehicle dynamics constraints, road speed limits, obstacle dynamics information, and ride comfort factors, a longitudinal speed distribution that satisfies both safety and smoothness is planned; the spatial trajectory from the path optimization results is then... As input, the planning period is discretized into a series of time steps. Using arc-length-time ST domain dynamic programming, the velocity curve is modeled as s(t) or The system is a discrete sequence at each time step t. k Below, combined with path point s k and obstacle prediction trajectory The distance between the vehicle and dynamic obstacles at the current and future moments is determined in real time to form a set of safety constraints; the speed planning objective function includes desired speed tracking, acceleration smoothing, obstacle safety distance, and vehicle longitudinal dynamics constraints;

[0051] The velocity planning can be formalized into the following optimization problem:

[0052]

[0053] where v k is the longitudinal speed of the vehicle at time t k , v ref (s k ) is the desired speed of the road section, is the acceleration, is the jerk, w v ,w a ,w j are weighting coefficients respectively; R safe (s k ,t k ) is the safety constraint penalty term;

[0054] The safety constraint term is defined as:

[0055]

[0056] where, represents the predicted longitudinal position of obstacle i at time t k , λ i is the safety penalty weight, and σ is the safety distance parameter;

[0057] The optimization process includes maximum / minimum speed, acceleration, and jerk constraints;

[0058] v min ≤v k ≤v max

[0059] |a k |≤a max

[0060] |j k |≤j max

[0061] The road speed limit and temporary speed constraints under special road sections are dynamically adjusted through the space-time mapping mechanism v ref (s k ) and related physical boundaries; the output longitudinal speed sequence and acceleration sequence corresponding to the spatial path are provided for the target speed instruction of downstream trajectory tracking and vehicle control.

[0062] In step S5, trajectory tracking and vehicle control execution includes: inputting the spatial path and longitudinal velocity sequence into the vehicle model predictive control system, calculating the optimal front wheel steering angle and acceleration control command in combination with the current state of the vehicle; the control objective is to minimize lateral, heading and speed errors and suppress abrupt changes in control quantities; and sending the execution command to the vehicle chassis controller in real time to achieve trajectory tracking.

[0063] In step S5, trajectory tracking and vehicle control execution specifically includes:

[0064] Receive optimized Frenet space path sequence and the corresponding target speed With acceleration Based on the current actual state of the vehicle (x) veh ,y veh ,ψ veh ,v veh The system maps the vehicle's actual position and attitude to the Frenet coordinate system of the reference path through real-time coordinate transformation, thereby calculating the lateral error, heading error, and longitudinal velocity error.

[0065] In the single-track model, the vehicle state variables are represented as x = [x, y, ψ, v], and the control variables include the front wheel steering angle δ and the longitudinal acceleration a. The single-step discrete dynamics model is as follows:

[0066] x k+1 =x k +v k cos(ψ k )Δt

[0067] y k+1 =y k +v k sin(ψ k )Δt

[0068]

[0069] v k+1 =v k +a k Δt

[0070] Among them, (x k ,y k ) represents the coordinates of the vehicle's center of mass, ψ k Let v be the heading angle. k Let L be the center of mass velocity, L be the vehicle wheelbase, and δ be the center of mass velocity. k For the front wheel steering angle, a k Let Δt be the longitudinal acceleration and Δt be the control period. The control objective is to minimize the deviation between the vehicle state and the reference trajectory within the finite prediction time domain T. The optimization objective function is as follows:

[0071]

[0072] where N p is the prediction step, N c is the control step, l i is the lateral offset of the vehicle's current position in Frenet coordinates, ψ i is the vehicle's heading, v i is the vehicle's speed, are the reference trajectory, heading and speed at the corresponding time, w d ,w ψ ,w v ,w δ ,w a are the weight coefficients;

[0073] The vehicle model predictive control system reconfigures the prediction trajectory and solves the optimal control sequence according to the current state of the vehicle in each cycle through rolling optimization, and only executes the first control amount, and the system sets constraints for the front wheel steering angle, longitudinal acceleration and its change rate as follows:

[0074] |δ i |≤δ max

[0075] |a i |≤a max

[0076] |δ i+1 -δ i |≤Δδ max

[0077] |a i+1 -a i |≤Δa max

[0078] where δ max and a max are the vehicle's maximum steering and acceleration capabilities, and Δδ max and Δa max are the upper limits of the change rate;

[0079] The calculated optimal front wheel steering angle and acceleration command are sent to the underlying actuator to realize the tracking of the spatial path and speed command by the vehicle.

[0080] Another object of the present application is to provide a city road trajectory planning and control system for regulating and controlling the city road trajectory planning and control method, which comprises:

[0081] A perception and environment modeling module for providing environment input for the trajectory interval generation module, the path optimization module and the trajectory tracking control module;

[0082] a trajectory interval generation module, configured to receive output information of the perception and environment modeling module, and dynamically construct a feasible trajectory region and provide a constraint boundary for a path optimization module in response to a current position of the vehicle, a road structure, and an obstacle distribution after receiving structured road and obstacle data based on a Frenet coordinate system;

[0083] a path optimization module, configured to receive the feasible trajectory region interval output by the trajectory interval generation module, and generate a spatially optimal path curve in combination with multiple optimization objectives of the vehicle;

[0084] a speed planning module, configured to plan a longitudinal speed distribution based on the spatially optimal path curve in combination with vehicle dynamics constraints, road speed limits, dynamic obstacle information, and ride comfort factors;

[0085] a trajectory tracking control module, configured to convert the spatial path and speed curve generated by the upstream path optimization and speed planning modules into real-time executable vehicle control instructions, and ensure that the vehicle tracks the expected trajectory.

[0086] In combination with all the technical solutions described above, the present application has the following beneficial effects:

[0087] First, in response to the requirements of dense obstacles and dynamic interaction in urban roads, the present application proposes an intelligent vehicle trajectory planning and optimization method based on real-time constraint trajectory intervals and a Frenet coordinate system. By mapping the spatial information of the vehicle and the environmental obstacles into the Frenet coordinate system in real time, a feasible trajectory interval reflecting the road structure and obstacle distribution is dynamically generated. In combination with high-order polynomial curve fitting for path points, multiple optimization objectives such as vehicle dynamics constraints, obstacle dynamic classification, smoothness, and safety distance are jointly introduced to realize the separation optimization and adaptive adjustment of the path and speed. The planning process updates the trajectory interval and optimization weight in real time within each control period, which can efficiently cope with complex obstacles, sudden traffic incidents, and changing road conditions in urban structural roads, and output vehicle driving trajectories that take into account physical feasibility, smoothness, and safety.

[0088] Second, the present application takes the Frenet coordinate system as the core innovation and integrates the following key technical breakthroughs:

[0089] (1) Dynamic trajectory interval generation based on real-time mapping of obstacles. This method can project and divide complex and dynamic obstacle information in urban roads into the Frenet space in real time, dynamically generating a feasible trajectory region that changes in real time. This innovation breaks through the limitation of previous planning methods that require pre-set obstacles or intervals, allowing the trajectory to always maintain high consistency with the road structure and obstacle environment.

[0090] (2) Trajectory interval and multi-dimensional optimization target collaborative modeling. Within the trajectory interval, the application uses high-order polynomial curves to fit the discrete path points, realizes high-order continuity of the path, and jointly introduces vehicle dynamics constraints, path smoothing, comfort, obstacle avoidance safety and other multiple optimization targets to ensure that the generated trajectory takes into account safety, comfort and practical feasibility.

[0091] (3) High adaptability and robustness to urban structured roads. In view of the characteristics of urban roads, such as variability, congestion, and obstacle density, the method of the application can dynamically adjust the trajectory interval and optimization weight according to the real-time feedback of the perception module, and improve the robustness to perception noise and prediction error through relaxation factor, effectively reducing the collision risk in urban traffic scenarios.

[0092] (4) Path and speed decoupling and collaborative optimization. The path-speed decomposition strategy is adopted to realize spatial and temporal domain separation optimization, which significantly improves the real-time performance of the algorithm and meets the high-frequency planning and control requirements.

[0093] Third, the application effectively overcomes the problems of existing trajectory planning methods, such as path infeasibility, poor real-time performance and insufficient safety in obstacle-dense, dynamic and complex road structure scenarios; by introducing dynamic feasible trajectory interval and multi-objective collaborative optimization, the autonomous decision-making ability of the vehicle in sudden obstacles, congested sections and complex urban intersections is significantly improved, ensuring the continuity, smoothness and physical feasibility of the trajectory; the system can adapt to perception errors and environmental changes, improving the response speed and robustness to dynamic traffic flow, greatly enhancing the safety, comfort and engineering application value of autonomous vehicles in actual urban road environments. The application provides a more practical trajectory decision-making core capability for intelligent driving systems in urban structured road environments, and can efficiently and safely complete trajectory planning and dynamic obstacle avoidance in complex scenarios such as urban intersections, narrow alleys, frequent temporary obstacles and irregular lanes, promoting the scale commercial application of autonomous vehicles and providing important technical support for intelligent city traffic management and urban travel safety.

[0094] Fourth, the perception-modeling-constraint-optimization efficient decoupling framework proposed by the application breaks through the bottlenecks of weak adaptability to obstacle-dense scenarios and dynamic response lag in previous technologies, realizing efficient and safe optimization under multiple targets. BRIEF DESCRIPTION OF DRAWINGS

[0095] The accompanying drawings, which are incorporated in and constitute a part of the specification, illustrate embodiments consistent with the present disclosure and, together with the description, serve to explain the principles of the present disclosure;

[0096] Figure 1 is the urban road trajectory planning and control principle flowchart provided by the embodiment of the application;

[0097] Figure 2 This is a schematic diagram of the global coordinate system transformation to Frenet coordinate system projection in an embodiment of the present invention; Detailed Implementation

[0098] To make the above-mentioned objects, features, and advantages of the present invention more apparent and understandable, specific embodiments of the present invention will be described in detail below with reference to the accompanying drawings. Many specific details are set forth in the following description to provide a thorough understanding of the present invention. However, the present invention can be practiced in many other ways different from those described herein, and those skilled in the art can make similar modifications without departing from the spirit of the present invention. Therefore, the present invention is not limited to the specific embodiments disclosed below.

[0099] The innovation of this invention lies in:

[0100] (1) The embodiments of the present invention adopt a multi-scenario adaptive feasible trajectory region generation strategy. The Frenet region boundary is constructed in real time in a single traversal through the extreme value-adaptive smoothing hybrid algorithm. The safe and feasible trajectory region is accurately updated in the narrow bottleneck scenarios caused by highway ramps, intersections and roadside parking. The upper and lower boundaries are updated in real time according to the sampling points and adaptively extended to the dynamic obstacle prediction trajectory. This overcomes the lag problem of the traditional static region method, provides reliable initial constraints for subsequent optimization, and significantly improves the robustness and timeliness of the system.

[0101] (2) The present invention introduces a spatial domain improved vehicle kinematics model and couples multiple steering constraints, transforming the traditional nonlinear vehicle model from the time domain to the spatial domain. Under the curvature-velocity relaxation mechanism, a smooth steering sequence is continuously generated, which effectively suppresses serpentine correction, reduces steering wheel angle fluctuations, and improves the comfort and safety margin of the vehicle during multi-segment steering in narrow roads.

[0102] (3) The embodiments of the present invention construct a multi-objective collaborative convex optimization framework. First, the collision constraint is the region boundary, and then the lateral offset, curvature penalty and dynamic risk Gaussian term are unified into an approximate convex quadratic objective function, which significantly reduces the solution complexity and is robust to perception noise, thus solving the real-time problem of lane narrowing-multi-obstacle scenarios.

[0103] (4) The embodiments of the present invention adopt path-velocity separation solution and coupled with model predictive control (MPC) closed loop, dynamically adjust the control quantity within a 100ms rolling cycle, solve the problem that traditional single-unit optimization is prone to curvature peaks and speed jitter in continuous multi-turn narrow road scenarios, avoid frequent replanning stagnation in temporary narrow roads, and ensure real-time response and smooth tracking of the system.

[0104] Example 1, as Figure 1 As shown, the urban road trajectory planning and control method provided in this embodiment of the invention includes the following steps:

[0105] S1, environment perception and modeling; data fusion of urban road environment based on multi-source perception devices; using spatial clustering algorithm to uniformly represent road elements and obstacles as structured objects and map them to Frenet coordinate system, generating Frenet projection of obstacle set and road physical boundary;

[0106] S2, path optimization and high-order curve fitting; in multiple scenarios, using adaptive feasible trajectory region generation strategy, using extreme value-adaptive smoothing hybrid algorithm to construct Frenet region boundary in real time in a single pass, for complex scenarios such as high-speed ramps, intersections and road-side parking induced narrow bottleneck, dynamically and accurately updating the safe and feasible trajectory region, and updating the upper and lower boundaries in real time according to the sampling points, and adaptively expanding to the dynamic obstacle prediction trajectory, overcoming the hysteresis problem of traditional static region method, providing reliable initial constraints for subsequent optimization, significantly improving system robustness and timeliness;

[0107] S3, path optimization and high-order curve fitting; based on spatial domain improved vehicle kinematics model, coupled with multiple steering constraints, the traditional nonlinear vehicle model is converted from time domain to spatial domain, and a smooth steering sequence is continuously generated under the curvature-speed relaxation mechanism, a multi-objective collaborative convex optimization framework is constructed. Collision constraints are converted into region boundaries, lateral offset, curvature penalty and dynamic risk Gaussian term are unified into an approximate convex quadratic objective function, real-time optimization of path variables, improving the comfort and safety margin of the vehicle in the process of multi-turning on narrow road;

[0108] S4, longitudinal speed and acceleration planning; using path-speed separation strategy and closed-loop coupling with model predictive control (MPC), generating expected speed and acceleration sequence for each sampling point based on spatial-time domain dynamic programming method, realizing expected speed tracking, acceleration smoothing and obstacle safety constraint, solving the problem of curvature spike and speed jitter in continuous multi-turning narrow road scenario, avoiding frequent re-planning stagnation in temporary narrow road;

[0109] S5, trajectory tracking and vehicle control execution: input the spatial path and longitudinal speed sequence to the vehicle model predictive control system to calculate the optimal front wheel steering angle and acceleration control command to realize vehicle trajectory tracking.

[0110] Example 2: The urban road trajectory planning and control system provided in this embodiment of the invention consists of a perception and environment modeling module, a trajectory interval generation module, a path optimization module, a speed planning module, and a trajectory tracking control module. The system first acquires real-time road structure and obstacle information through the perception and environment modeling module and maps it to the Frenet coordinate system. The trajectory interval generation module dynamically divides feasible trajectory regions based on the spatial relationship between the vehicle and obstacles, forming constrained trajectory intervals. The path optimization module continuously fits discrete sampling points within the trajectory interval using high-order polynomial curves, and collaboratively considers vehicle dynamics constraints and path smoothness to achieve path optimization. The speed planning module adjusts the longitudinal speed distribution based on the optimized path to improve driving safety and comfort. Finally, the trajectory tracking control module sends the optimized path and speed commands to the vehicle for execution, achieving precise control of the vehicle's motion state. The close collaboration among these modules ensures that the system can output physically feasible, continuously smooth, and safe vehicle driving trajectories in complex urban road scenarios.

[0111] The perception and environment modeling module is the primary foundational component of this invention. Its function is to provide precise and structured environmental input for the trajectory interval generation module and subsequent path optimization and control modules. This module performs data fusion on the urban road environment based on multi-source sensing devices (such as LiDAR, cameras, millimeter-wave radar, and high-precision maps). First, it performs high-precision detection and positioning of road structures, lane lines, traffic signs, static obstacles (such as parked vehicles, guardrails, and road facilities), and dynamic obstacles (such as pedestrians, non-motorized vehicles, and other vehicles). To adapt to the changes in complex urban road structures, this module innovatively adopts a multi-objective fusion spatial clustering algorithm and density estimation algorithm to uniformly represent the original perceived point cloud and image information as an obstacle set O in a global coordinate system. The obstacle set O can be denoted as: O = {o i |o i =(x i ,y i ,l i ,w i ,v xi ,v yi )}, where x i ,y i Let l be the global spatial position of obstacle i, and l be the global spatial position of obstacle i. i ,w i v represents the length and width of the obstacle. xi ,v yi This represents the velocity components of the obstacle in the global coordinate system.

[0112] like Figure 2As shown, the module maps the obstacle set and road structure information to the Frenet coordinate system through projection transformation to achieve efficient constraint modeling in the trajectory planning process. The Frenet coordinate system takes the global reference line (road centerline or preset path) as the reference, defines the longitudinal s-axis (in the direction of the reference line arc length) and the lateral d-axis (perpendicular to the reference line direction), and performs coordinate transformation on each obstacle point (x i ,y i ). Specifically, the reference line is represented as ref = {(x ref (s), y ref (s))} in the global coordinate system, and the longitudinal projection s i of the obstacle point is obtained by minimizing the Euclidean distance:

[0113]

[0114] The lateral offset l i is calculated by the normal vector of the obstacle point and the projection point;

[0115] l i = [(x i , y i ) - (x ref (s i ), y ref (s i ))] · n(s i )

[0116] Where n(s i ) is the unit normal vector of the reference line at s i , obtained by vertical transformation of the reference line tangent vector t(s i );

[0117] To realize real-time modeling of dynamic obstacles, the system predicts the future time domain trajectory of dynamic obstacles based on multi-model state estimation algorithms such as Kalman filtering, generates a spatiotemporal prediction set Where T is the planning prediction time domain. The trajectory prediction result of the dynamic obstacle is also projected at each time in the Frenet coordinate, providing a boundary basis for subsequent trajectory interval generation and risk modeling.

[0118] In view of the complex road structure changes and environmental perception uncertainty, the module innovatively introduces a density-adaptive DBSCAN clustering algorithm to process the spatial clustering of the perception data, thereby improving the obstacle classification accuracy in dense obstacle scenarios. Meanwhile, through a sliding window weighted probability data fusion mechanism, the perception noise and occasional false detection are effectively alleviated, thereby improving the robustness and stability of the environmental modeling. In addition, the module discriminates the obstacle types and traffic rules, and describes different obstacles such as vehicles, pedestrians and temporary road structures in the form of a polygon or a convex hull boundary at the model level, thereby providing a high-fidelity environmental constraint description for each stage of the trajectory planning. Finally, the module outputs all the obstacles, lane lines and traffic elements in the form of structured Frenet space constraint data, thereby providing real-time and accurate environmental basic data support for the subsequent trajectory interval generation, path optimization and speed planning modules, and ensuring that the overall system has high-precision, high-adaptability and high-robustness perception modeling capabilities in complex urban road environments.

[0119] The trajectory interval generation module closely receives the output information of the perception and environmental modeling module, and is a core link for realizing efficient and safe trajectory optimization of the present application. After receiving the structured road and obstacle data based on the Frenet coordinate system, the module needs to dynamically construct the feasible trajectory region according to the current position of the vehicle, the road structure and the obstacle distribution, and provide the constraint boundary for the subsequent path optimization module. First, according to the reference path of the vehicle in the planning period, the longitudinal arc length interval [s0, sT] is discretized into an equidistant sampling point sequence N Each sampling point corresponds to a set of lateral interval boundary parameters.

[0120] At each sampling point s k , the system retrieves all the obstacle and road boundary information at this longitudinal position, and generates the lower bound and the upper bound based on the obstacle coordinates (s i , d i ) obtained by the projection of the perception module through the maximum-minimum extreme value method, and the specific calculation is as follows:

[0121] For o i in the obstacle set O, if the longitudinal projection s i satisfies |s i -s k |<∈ (where ∈ is the distance tolerance), the lateral position d i can be regarded as the constraint point of the current sampling point s k . Based on the road physical boundary , the maximum value of all lower constraint points is taken as the lower bound of the current lower feasible trajectory interval, and the minimum value of all upper constraint points is taken as the upper bound of the current upper feasible trajectory interval, which is denoted as:​

[0122]

[0123] in, Indicates in s k A set of obstacle indices located near the vehicle's left or below. For the set of indices of obstacles on the right or above, l lb (s k ) and l ub (s k The original lower / upper boundaries are obtained using the extreme value method. Through the above extreme value operation, it can be ensured that the trajectory interval fully utilizes the road space while avoiding collisions with obstacles. To improve the algorithm's adaptability to dense obstacles and dynamic traffic scenarios, this module introduces an adaptive interval smoothing algorithm to smooth the initially generated boundary sequence {d}. lb (s k )},{d ub (s k The moving average filter is applied to reduce abrupt changes in trajectory intervals caused by sensor noise or false detections in a single frame. Specifically, the processing is as follows:

[0124] Define the window W (length M sampling points) and the adaptive weights;

[0125] ω k =exp(-|l b (s k )-l b (s k-1 )| / σ)

[0126] Among them, l b (s k ) represents the edge sampling points s of the feasible trajectory region. k The constraint point, ω k And σ adaptive smoothing weight and sensitivity parameter;

[0127] For the initially generated boundary sequence {l lb (s k )},{l ub (s k Apply moving average filtering;

[0128]

[0129] Where M is the length of the smoothing window. For dynamic obstacles, to ensure the feasibility and robustness of trajectory planning, the module also maps the predicted trajectory of dynamic obstacles and participates in the calculation of the interval boundary. If the obstacle has a velocity component, its future projection point in the planning time domain is predicted, and the interval boundary is updated with the most unfavorable value of the lateral position at all times, thereby ensuring that the entire trajectory interval is always safe and reliable.

[0130] In addition, for special urban road structures (such as variable lanes, road narrowing or merging, temporary closed areas, etc.), the system will adjust the parameters of the physical boundary in real time according to the high-precision map or online perception, and realize adaptive matching with the actual road environment. Finally, this module outputs the feasible trajectory interval of each sampling point in the interval sequence , providing spatial constraints for the subsequent path optimization module. This mechanism not only ensures that the vehicle trajectory is always in a feasible and safe area, but also provides a solid foundation for efficient and smooth path optimization and obstacle avoidance.

[0131] The path optimization module takes the feasible trajectory region interval output by the trajectory interval generation module as the input, and is the key link to realize the safety, smoothness and physical feasibility of vehicle motion. The task of this module is to generate an optimal path curve based on the feasible lateral interval of each sampling point in the known Frenet coordinate system , combined with multiple optimization objectives such as vehicle dynamics constraints, trajectory continuity and smoothness. First, the path of the entire planning interval is modeled as a set of discrete lateral offset variables , where each l k represents the lateral offset at the longitudinal sampling point s k , satisfying the spatial interval constraint To make the path meet the smoothness and dynamic feasibility, high-order polynomials (such as quintic polynomials) or spline curves are used to fit the path. Taking the quintic polynomial modeling as an example, each path segment on the interval [s k , s k+1 ] can be expressed as:

[0132] l(s) = a0 + a1(s-s k ) + a2(s-s k ) 2 + a3(s-s k ) 3 + a4(s-s k ) 4 + a5(s-s k ) 5

[0133] Where a0,…,a5 are polynomial coefficients, and the global continuous and differentiable curve is constructed by constraining the continuity of the value, derivative and second derivative of the path at the sampling points.

[0134] The path optimization objective function is expressed in terms of polynomial coefficients or discrete point sets {l k ​}is the decision variable, the typical objective function includes three parts: (1) trajectory smoothness (achieved by suppressing the lateral second derivative, i.e. the change of curvature), (2) deviation from the reference path (e.g. the road centerline), (3) obstacle risk penalty term and vehicle dynamics feasibility constraints. The specific objective function can be expressed as:

[0135]

[0136] where Δs = s k+1 -s k is the sampling interval, l ref (s k ) is the lateral position of the reference line at s k , w1, w2 are weight coefficients, R obs (l k , s k ) is the obstacle risk penalty term, which weights and suppresses the path close to obstacles. The first term is used to guarantee the smoothness of the path, limiting the curvature and the rate of change of curvature; the second term controls the deviation of the path from the road center, reducing unnecessary deviation; the third term introduces a risk factor for perceiving obstacles, protecting against uncertainty of dynamic obstacles or boundaries, and usually uses a Gaussian penalty function:

[0137]

[0138] where, is the set of obstacles near s k , λ i is the penalty weight of obstacle i, σ i represents the uncertainty of the obstacle boundary.

[0139] To ensure the dynamics feasibility of the vehicle trajectory, this module constrains the first and second derivatives of the path, i.e. gives physical upper limits to the lateral velocity and acceleration of the path. If a polynomial parameterization model is used, all interval coefficients of the global curve are optimized through boundary conditions, so that the path satisfies the continuity requirements of the actual vehicle state at the start and end point positions, velocities, accelerations, etc. In addition, through slack variables and quadratic programming algorithms, the path optimization process can still maintain convergence and robustness when there is a small conflict in the boundary or perception noise. The entire optimization problem is finally constructed as a convex quadratic programming problem or a nonlinear programming problem with linear and quadratic constraints, and is solved in real time by an efficient numerical solver within each planning period to obtain the optimal path.

[0140] This path optimization module not only guarantees the physical safety and motion smoothness of the vehicle within the feasible interval, but also greatly improves the real-time performance and robustness in a multi-obstacle, high-dynamic urban road environment, providing a continuous, smooth and dynamically constrained ideal target path for subsequent speed planning and motion control links.

[0141] The speed planning module, as the direct downstream of the path optimization module, mainly aims to plan a longitudinal speed profile that satisfies safety and smoothness based on the determined spatial optimal path curve, combined with vehicle dynamics constraints, road speed limits, dynamic information of obstacles, and ride comfort, etc. The module first takes the spatial trajectory in the path optimization result as input, and discretizes the planning period into a series of time steps On this basis, the S-T (arc length-time) domain dynamic programming is adopted to model the speed curve as a discrete sequence of s(t) or At each time step t k , the module combines the path point s k and the predicted trajectory of obstacles to determine the distance between the vehicle and dynamic obstacles at the current and future time, and forms a set of safety constraints. The speed planning objective function usually contains multiple items, including expected speed tracking, acceleration smoothing, obstacle safety distance, and vehicle longitudinal dynamics constraints.

[0142] The speed planning can be formalized as the following optimization problem:

[0143]

[0144] where v k is the longitudinal speed of the vehicle at time t k , v ref (s k ) is the expected speed of the road section, a is the acceleration, j is the jerk, w v , w a , and w j are the weighting coefficients of each item. R safe (s k , t k ) is the safety constraint penalty term, which is used to ensure that the vehicle always maintains a safe distance from dynamic obstacles at all time steps, and is usually expressed by a Gaussian function or a piecewise function expression, which greatly increases the penalty strength when the distance is less than a threshold. For example, the safety constraint term can be defined as:

[0145]

[0146] where s k represents the predicted longitudinal position of obstacle i at time t i , λ min is the safety penalty weight, and σ is the safety distance parameter.

[0147] ​The optimization process is subject to vehicle dynamics constraints, including maximum / minimum speed, acceleration, and jerk constraints, expressed as:

[0148] v min ≤v k ≤v max ,|a k |≤a max ,|j k |≤j max

[0149] In addition, road speed limits and temporary speed constraints under special road sections (such as curves, narrow areas) can be dynamically adjusted through the space-time mapping mechanism v ref (s k ) and related physical boundaries.

[0150] To achieve adaptive response to sudden obstacles and high-density traffic environments, this module integrates dynamic obstacle trajectory prediction and uncertainty modeling, incorporates the uncertainty interval of obstacle prediction into the safety penalty function, and adaptively adjusts the weight parameter in each planning cycle to improve the robustness and real-time performance of the planning result. For the processing of traffic signals, temporary speed limits, and special events, the system dynamically modifies the expected speed curve based on the high-precision map and perception module feedback to ensure that the vehicle complies with traffic regulations, further improving the engineering adaptability and actual safety of the overall system.

[0151] Finally, the speed planning module outputs a longitudinal speed sequence and an acceleration sequence corresponding to the spatial path, providing continuous, smooth, and dynamically feasible target speed instructions for downstream trajectory tracking and vehicle control modules, thereby achieving safe, efficient, and comfortable operation of the vehicle in complex urban road environments.

[0152] The trajectory tracking control module, as the end execution link of the system, is responsible for converting the spatial path and speed curve generated by the upstream path optimization and speed planning modules into real-time executable vehicle control instructions, ensuring that the vehicle tracks the expected trajectory with high precision and robustness, and adapts to the changing conditions in complex urban road environments. This module first receives the optimized Frenet space path sequence and its corresponding target speed and acceleration , combined with the current actual state of the vehicle (x veh , y veh , ψ veh , v veh ), through real-time coordinate transformation to map the actual position and attitude of the vehicle to the Frenet coordinate system of the reference path, thereby calculating the lateral error, heading error, and longitudinal speed error to provide a state basis for control decisions.

[0153] The control algorithm is based on a nonlinear vehicle dynamics model and a model predictive control (MPC) framework, fully considering practical dynamic constraints such as the vehicle's steering, acceleration, deceleration capabilities, tire side slip characteristics, and stability under low-speed, high-curvature conditions. Taking the bicycle model as an example, the vehicle state variables can be expressed as x = [x, y, ψ, v], and the control variables include the front wheel steering angle δ and the longitudinal acceleration a. The single-step discrete dynamics model is as follows:

[0154] x k+1 =x k +v k cos(ψ k )Δt

[0155] y k+1 =y k +v k sin(ψ k )Δt

[0156]

[0157] v k+1 =v k +a k Δt

[0158] Among them, (x k ,y k ) represents the coordinates of the vehicle's center of mass, ψ k Let v be the heading angle. k Let L be the center of mass velocity, L be the vehicle wheelbase, and δ be the center of mass velocity. k For the front wheel steering angle, a k Let Δt be the longitudinal acceleration and Δt be the control period. The control objective is to minimize the deviation between the vehicle state and the reference trajectory within the finite prediction time domain T. A typical optimization objective function is as follows:

[0159]

[0160] Where, N p To predict the number of steps, N c To control the number of steps, l i ψ represents the lateral offset of the vehicle's current position in Frenet coordinates. i For vehicle heading, v i For vehicle speed, These represent the reference trajectory, heading, and velocity at the corresponding time points, w d ,w ψ ,w v ,w δ ,w a These are the weighting coefficients for each component. This objective function not only optimizes the vehicle's tracking error along the path but also suppresses drastic changes in control variables, improving ride comfort and system robustness.

[0161] MPC reconfigures the predicted trajectory and solves the optimal control sequence according to the current state of the vehicle in real time by rolling optimization in each cycle, and only the first control amount is executed. To ensure that the control instruction is physically feasible, the system sets constraints on the front wheel steering angle, longitudinal acceleration and its rate of change:

[0162] |δ i |≤δ max ,|a i |≤a max ,|δ i+1 -δ i |≤Δδ max ,|a i+1 -a i |≤Δa max

[0163] Where δ max and a max are the vehicle's maximum steering and acceleration capabilities, and Δδ max and Δa max are the upper limits of the rate of change.

[0164] For common scenarios such as sharp turns, narrow lanes or dense obstacles on urban roads, the system automatically increases the lateral error and heading error weights to improve trajectory accuracy; in high-speed straight environments, focus on optimizing speed tracking and smoothness. To deal with perception errors and execution delays, the module introduces an observer and a delay compensation algorithm to filter and feedforward predict the vehicle's state, thereby improving tracking accuracy and robustness.

[0165] Finally, the trajectory tracking control module will issue the optimal front wheel steering angle and acceleration instructions calculated to the underlying execution mechanism, achieving high-precision, dynamic response tracking of the vehicle to spatial path and speed instructions, ensuring safe, smooth and efficient operation of the vehicle on complex urban road environments, and providing a solid guarantee for the practicality and engineering promotion of the entire system.

[0166] In summary, the present application proposes a kind of urban road automatic driving intelligent trajectory planning and control method based on real-time constraint trajectory interval and Frenet coordinate system, which aims to realize the efficient planning and accurate control of safe, smooth and dynamic feasible trajectory of automatic driving vehicle in urban structural road environment through environment perception and modeling, dynamic generation of feasible trajectory interval, path optimization and high-order curve fitting, longitudinal speed and acceleration planning, trajectory tracking and vehicle control execution etc. Core modules, significantly improve the safety, comfort and system robustness of the vehicle in complex traffic environment through multi-source perception information fusion, dynamic constraint modeling based on Frenet space and optimization decision of whole process, promote the engineering application and large-scale landing of automatic driving technology in actual urban road scene.

[0167] From the perspective of theoretical analysis and engineering test, the application realizes the fundamental improvement of real-time response capability and physical feasibility by introducing a multi-scene adaptive Frenet trajectory corridor generation strategy and a spatial domain dynamics modeling. Based on the extreme value-adaptive smoothing hybrid algorithm, the system can adaptively adjust the feasible trajectory corridor in milliseconds in response to the dynamic changes of obstacle distribution and road structure in complex urban road environment, effectively overcoming the problems such as trajectory interval update lag, path infeasibility and control non-smoothness of the prior art in the face of dynamic obstacles, temporary narrow lanes, high-speed ramps and roadside temporary parking and other special scenes. Secondly, the application greatly reduces the optimization complexity and avoids the problem of difficult coordination of traditional multi-objective weights by first corridor convexity (changing collision to boundary constraint) and then approximating all second-order and dynamic risk items to convex quadratic objectives. In theory, when the perception noise and obstacle prediction error increase, the trajectory optimization can still guarantee the feasibility of the solution and will not appear the situation of infeasible solution or frequent trajectory jumping. At the same time, the combination of the spatial domain dynamics model and the multi-objective convex optimization framework enables the trajectory optimization to not only consider vehicle physical constraints, path smoothness and multi-steering comfort, but also realize high-frequency dynamic adjustment through path-speed separation and MPC closed-loop rolling mechanism, significantly improving the tracking stability and system robustness of the vehicle in continuous multi-steering and complex traffic flow.

[0168] From the perspective of designing experiments and expected experimental results, the comparison tests are carried out by analogy with the existing CARLA simulation and physical vehicle experimental environment, and typical urban road congestion, intersection area, temporary obstacle dense and other scenes are used as verification objects. The embodiment of the application is expected to be superior to the method based on static Frenet corridor or traditional A* / RRT in terms of trajectory planning delay, path smoothness, safety distance, trajectory feasibility and control instruction stability. Under actual complex working conditions such as high perception noise and dynamic obstacle prediction error, the scheme can still stably output physically feasible and comfortable trajectories, and the problems such as planning stagnation, frequent re-planning and insufficient safety distance are greatly reduced, fully showing the significant advantages and engineering practicability of the technical scheme of the application in the process of research and application.

[0169] The above is only a preferred specific embodiment of the application, but the protection scope of the application is not limited thereto, and any modification, equivalent replacement and improvement made by those skilled in the art within the technical scope disclosed by the application and within the spirit and principles of the application shall be covered within the protection scope of the application.

Claims

1. A method for urban road trajectory planning and control, characterized in that, The method comprises the following steps: S1, environment perception and modeling; data fusion of urban road environment based on multi-source perception equipment; road elements and obstacles are uniformly represented as structured objects by using a spatial clustering algorithm and mapped to a Frenet coordinate system to generate Frenet projections of obstacle sets and road physical boundaries; S2, path optimization and high-order curve fitting; in multiple scenarios, an adaptive feasible trajectory region generation strategy is adopted, an extreme value-adaptive smoothing hybrid algorithm is used to construct Frenet region boundaries in a single iteration, and the safe and feasible trajectory region is dynamically updated for complex scenarios such as high-speed ramps, crossroads and narrow bottleneck caused by roadside parking, and the upper and lower boundaries are updated in real time according to the sampling points, and the adaptive extension is extended to the predicted trajectory of the dynamic obstacle; S3, path optimization and high-order curve fitting; based on the improved vehicle kinematics model in the spatial domain, the multi-steering constraint is coupled, the nonlinear vehicle model is converted from the time domain to the spatial domain, and the smooth steering sequence is continuously generated under the curvature-speed relaxation mechanism, and a multi-objective collaborative convex optimization framework is constructed; the collision constraint is converted into a region boundary, the lateral offset, curvature penalty and dynamic risk Gaussian term are unified into an approximate convex quadratic objective function, and the path variables are optimized in real time; S4, longitudinal speed and acceleration planning; a path-speed separation strategy is adopted and coupled with model predictive control in a closed loop, and the expected speed and acceleration sequence of each sampling point is generated based on the space-time domain dynamic programming method, so that the expected speed tracking, acceleration smoothing and obstacle safety constraint are realized; S5, trajectory tracking and vehicle control execution: the spatial path and longitudinal speed sequence are input into the vehicle model predictive control system to calculate the optimal front wheel steering angle and acceleration control command to realize vehicle trajectory tracking.

2. The urban road trajectory planning and control method of claim 1, wherein, In step S1, the road structure, lane line, traffic sign, static obstacle and dynamic obstacle are detected and positioned by the multi-source perception equipment; a multi-object fusion spatial clustering algorithm and a density estimation algorithm are used to uniformly represent the original perception point cloud and image information in the global coordinate system as an obstacle set O, denoted as: O = {o i |o i = (x i , y i , l i , w i , v xi , v yi ) where o i is the attribute tuple of the obstacle O, x i ,y i are the global spatial position of the obstacle i, l i ,w i are the length and width of the obstacle, v xi ,v yi are the velocity components of the obstacle in the global coordinate system; The obstacle set and the road structure information are mapped to a Frenet coordinate system through a projection transformation; the Frenet coordinate system takes a global reference line as a reference to define a longitudinal s-axis and a lateral l-axis, and performs coordinate transformation on each obstacle point (x i ,y i ): The reference line is expressed in the global coordinate system as: ref = {(x ref (s),y ref (s))}, the longitudinal projection s i is obtained by minimizing the Euclidean distance, expressed as: Lateral offset l i This is calculated by the normal vector of the obstacle point and the projection point. l i =[(x i ,y i )-(x ref (s i ),y ref (s i ))]·n(s i ) where n(s i ) is the unit normal vector of the reference line at s i , obtained by a normal transformation of the reference line tangent vector t(s i ); Based on the multi-model state estimation algorithm including Kalman filter, the future time domain trajectory of dynamic obstacles is predicted to generate the spatio-temporal prediction set: Wherein, T is the planning prediction time domain; the trajectory prediction result of the dynamic obstacle is projected to each time in Frenet coordinates.

3. The urban road trajectory planning and control method of claim 2, wherein, The multi-source perception equipment includes a laser radar, a camera, a millimeter wave radar and a high-precision map; the static obstacle includes a parked vehicle, a guardrail and a road facility; the dynamic obstacle includes a pedestrian and a non-motor vehicle; the global reference line is a road center line or a preset path, and the s-axis is along the reference line arc length direction and the l-axis is perpendicular to the reference line direction.

4. The urban road trajectory planning and control method of claim 1, wherein, In step S2, the dynamic generation of the feasible trajectory interval includes: discretizing the longitudinal path into multiple sampling points according to the current position of the vehicle and the planning interval; the extreme value method and the adaptive smoothing algorithm are used to dynamically calculate the corresponding lateral feasible interval according to the obstacle projection and road boundary information at each sampling point, the feasible trajectory region covering the entire planning section is constructed, and the interval constraint extension is simultaneously performed on the future predicted trajectory of the dynamic obstacle.

5. The urban road trajectory planning and control method of claim 1, wherein, In step S2, the dynamic generation of the feasible trajectory interval specifically includes: According to the reference path of the vehicle in the planning period, the longitudinal arc length interval [s0, s N ] is discretized into an equidistant sequence of sampling points Each sampling point corresponds to a set of lateral interval boundary parameters; At each sampling point s k , the system retrieves all the obstacle and road boundary information at this longitudinal position and generates the lower and upper boundaries based on the obstacle coordinates (s i , l i ) projected by the perception module using the max-min extreme value method, as follows: For all obstacles o in the set O i , if the longitudinal projection s i satisfies |s i -s k | < ∈, where ∈ is a distance tolerance, then the lateral position l i can be considered as a constraint point of the current sampling point s k ; based on the physical boundary of the road , the maximum value of all lower constraint points is taken as the lower bound of the current lower feasible trajectory interval, and the minimum value of all upper constraint points is taken as the upper bound of the current upper feasible trajectory interval; wherein, represents the obstacle index set located on the left or below the vehicle near the s k position, is the right or above obstacle index set, l lb (s k ) and l ub (s k ) are the original lower / upper bounds obtained by the extreme value method. An adaptive interval smoothing algorithm is introduced, and a window W and an adaptive weight are defined; ω k = exp(-|l b (s k )-l b (s k-1 )| / σ) wherein, l b (s k ) is a constraint point of the edge sampling point s k of the feasible trajectory region, ω k and σ are adaptive smoothing weight and sensitivity parameters; The boundary sequence {l lb (s k )},{l ub (s k )} generated initially is applied with a sliding mean filter; wherein, and the smoothed and extended final feasible interval upper and lower bounds, M is the smoothing window length; The dynamic obstacle prediction trajectory is mapped and participates in the calculation of the interval boundary, if the obstacle has a speed component, the future projection point in the planning time domain is predicted, and the most unfavorable value of the lateral position at all times is taken to update the interval boundary; For unique urban road structures, physical boundaries are adjusted in real time based on high-precision maps or online sensing. The parameters are used to achieve adaptive matching with the actual road environment, and the feasible trajectory interval of each sampling point is sequenced into intervals. The output includes special urban road structures such as: reversible lanes, road narrowing or merging, and temporary closure areas.

6. The urban road trajectory planning and control method of claim 1, wherein, In step S3, the feasible lateral interval of each sampling point under the Frenet coordinate system is taken as the constraint condition The optimal path curve is generated by combining the vehicle dynamics constraint, trajectory continuity, smoothness and obstacle risk penalty term; the path of the entire planning interval is modeled as a set of discrete lateral offset variables where each l k represents the lateral offset at the longitudinal sampling point s k , satisfying the spatial interval constraint A high-order polynomial or spline curve is used to fit the path, and each path is represented on the interval [s k , s k+1 ] as: l(s) = a0 + a1(s-s k ) + a2(s-s k ) 2 + a3(s-s k ) 3 + a4(s-s k ) 4 + a5(s-s k ) 5 Wherein, a0,…,a5 are polynomial coefficients, by constraining the continuity of the values, derivatives and second-order derivatives of the path at the sampling points, a globally continuous and differentiable curve is constructed by concatenating each interval; The path optimization objective function takes polynomial coefficients or discrete point sets {l k} as decision variables, and is expressed as: where Δs = s k+1 -s k is the sampling interval, l ref (s k ) is the lateral position of the reference line at s k , w1, w2 are weight coefficients, R obs (l k , s k ) is the obstacle risk penalty term, which weights and inhibits the path close to the obstacle; the third term introduces the risk factor of perceiving the obstacle, and a Gaussian type penalty function is adopted: where, is the set of obstacles in the vicinity of s k is the set of obstacles in the vicinity of s i is the penalty weight of obstacle i, σ i denotes the uncertainty of the obstacle boundary.

7. The urban road trajectory planning and control method of claim 1, wherein, In step S4, the longitudinal velocity and acceleration planning includes: Based on the spatial optimal path curve determined in step S3, a longitudinal speed distribution satisfying safety and smoothness is planned in combination with vehicle dynamics constraints, road speed limits, dynamic information of obstacles and ride comfort factors; the spatial trajectory in the path optimization result is discretized into a series of time steps as input and the planning period is discretized into a series of time steps The speed curve is modeled as s(t) or a discrete sequence; at each time step t k , the path point s k and the predicted trajectory of the obstacle are combined to determine the distance between the vehicle and the dynamic obstacle at the current and future time, and a safety constraint set is formed; the speed planning objective function includes expected speed tracking, acceleration smoothing, obstacle safety distance and vehicle longitudinal dynamics constraints; The velocity planning is formalized as the following optimization problem: wherein v k is the longitudinal speed of the vehicle at time t k , v ref (s k ) is the road segment desired speed, is the acceleration, is the jerk, w v , w a , w j are weighting coefficients, respectively; R safe (s k , t k ) is the safety constraint penalty term; The safety constraint term is defined as: wherein, represents the predicted longitudinal position of the obstacle i at time t k , λ i is a safety penalty weight, and σ is a safety distance parameter; The optimization process includes maximum / minimum speed, acceleration and acceleration constraints; v min ≤v k ≤v max |a k |≤a max |j k |≤j max Road speed limit and temporary speed constraint under special road section, dynamically adjust v through space-time mapping mechanism ref (s k ) and related physical boundaries; output longitudinal speed sequence and acceleration sequence corresponding to the spatial path, provide target speed instruction for downstream trajectory tracking and vehicle control.

8. The urban road trajectory planning and control method of claim 1, wherein, In step S5, the trajectory tracking and vehicle control execution includes: inputting the spatial path and longitudinal velocity sequence to the vehicle model predictive control system, combining the current state of the vehicle, calculating the optimal front wheel steering angle and acceleration control command; the control objective is to minimize the lateral, heading and speed errors, and to suppress the mutation of the control amount; the command is executed in real time to the vehicle chassis controller to realize trajectory tracking.

9. The urban road trajectory planning and control method of claim 1, wherein, In step S5, the trajectory tracking and vehicle control execution specifically includes: Receiving an optimized frenet space path sequence And a corresponding target speed With acceleration Combined with the current actual state (x veh ,y veh ,ψ veh ,v veh ) of the vehicle, the actual position and attitude of the vehicle are mapped into the frenet coordinate system of the reference path through real-time coordinate transformation, so as to calculate the lateral error, the heading error and the longitudinal speed error; In the single-track model, the vehicle state variable is expressed as x=[x,y,ψ,v], the control amount includes the front wheel steering angle δ and the longitudinal acceleration a, and the single-step discrete dynamic model is: x k+1 = x k + v k cos(ψ k )Δt y k+1 = y k + v k sin(ψ k )Δt v k+1 = v k + a k Δt where (x k ,y k ) is the vehicle mass center coordinate, ψ k is the heading angle, v k is the mass center velocity, L is the vehicle wheelbase, δ k is the front wheel steering angle, a k is the longitudinal acceleration, and Δt is the control period. The control objective is to minimize the deviation of the vehicle state from the reference trajectory within a limited prediction time domain T, and the optimization objective function is as follows: where N p is the prediction step, c is the control step, i is the lateral offset of the vehicle's current position in Frenet coordinates, ψ i is the vehicle's heading, i is the vehicle's speed, are the reference trajectory, heading and speed at the corresponding time, respectively, w d ,w ψ ,w v ,w δ ,w a are the weight coefficients; The vehicle model predictive control system reconstructs the prediction trajectory and solves the optimal control sequence in real time according to the current state of the vehicle at each cycle, and only executes the first control amount, and the system sets constraints for the front wheel steering angle, longitudinal acceleration and its change rate as follows: | δ i | ≤ δ max |a i |≤a max | δ i+1 - δ i | ≤ Δδ max |a i+1 -a i |≤Δa max where δ max and a max are the vehicle limit steering and acceleration capabilities, Δδ max and Δa max are the upper limits on the rate of change. The optimal front wheel steering angle and acceleration command calculated is sent to the underlying actuator to realize the tracking of the spatial path and speed command by the vehicle.

10. An urban road trajectory planning and control system, characterized in that, The system is used for regulating and controlling the urban road trajectory planning and control method of any one of claims 1-9, and the system comprises: A perception and environment modeling module for providing environment input for the trajectory interval generation module, the path optimization module, and the trajectory tracking control module; A trajectory interval generation module for receiving structured road and obstacle data based on the Frenet coordinate system, dynamically constructing a feasible trajectory region for the vehicle current position, road structure and obstacle distribution, and providing a constraint boundary for the path optimization module; A path optimization module for receiving the feasible trajectory region interval output by the trajectory interval generation module, generating a spatial optimal path curve in combination with the vehicle multiple optimization objectives; A velocity planning module for planning a longitudinal velocity distribution based on the spatial optimal path curve, in combination with vehicle dynamics constraints, road speed limits, dynamic obstacle information, and ride comfort factors; A trajectory tracking control module for converting the spatial path and velocity curve generated by the upstream path optimization and velocity planning modules into real-time executable vehicle control commands to ensure that the vehicle tracks the desired trajectory.

Images