Graph-free crossroad smooth control method and system based on 4D millimeter wave radar

Abstract

The invention relates to a graph-free crossroad smooth control method and system based on a 4D millimeter wave radar. On the premise of not depending on a high-precision map and a V2X facility, through self-adaptive ground segmentation of longitudinal segmentation and gradient continuity constraint and in combination with boundary point judgment and geometric fitting of multi-dimensional feature fusion, an accurate static boundary model of a travelable area of the crossroad is reconstructed in real time; a static boundary is used as a hard constraint, a dynamic obstacle prediction trajectory is used as a safety constraint, a linear time-varying model prediction control problem with motion smoothness as a primary optimization target is constructed, and the linear time-varying model prediction control problem is converted into quadratic programming for real-time solution; besides, a closed-loop adaptive mechanism based on a smoothness index is also introduced, perception-control collaborative optimization is realized through multi-hypothesis generation and micro-programming forward simulation, and a geometric memory bank is configured for bottom control and experience reuse, so that all-weather, low-cost and high-smoothness graph-free crossroad automatic driving traffic capacity is effectively realized.

Description

Technical Field

[0001] This invention relates to the field of autonomous driving, specifically to a method, system, device, processor, and computer-readable storage medium for smooth control of mapless intersections based on 4D millimeter-wave radar. Background Technology

[0002] Intersections are a core challenge scenario for autonomous driving. Existing traffic management solutions mainly rely on the following types of technologies, but all of them have fundamental flaws: (1) Scheme based on high-precision prior maps: This scheme relies on pre-made centimeter-level high-precision maps for positioning and planning. Its fundamental flaw is that the system performance is highly dependent on the accuracy and real-time performance of the map. When faced with common changes such as road construction and temporary detours, the map and the actual objects will mismatch, which will directly lead to positioning drift, planning failure, and serious safety risks. This is essentially a contradiction between static prior information and the dynamic real world.

[0003] (2) Vehicle-to-everything (V2X) based solution: This solution obtains "God's-eye view" information through roadside equipment. Its fundamental flaw lies in binding the vehicle's safe passage capability to the deployment density and communication reliability of external infrastructure. This not only results in high construction costs and difficulty in widespread adoption, but also means that vehicles will lose critical perception capabilities when communication is interrupted or interfered with. Essentially, this is a risk to reliability and robustness arising from the externalization of autonomous decision-making power.

[0004] (3) Lightweight map / mapless solution based on traditional vehicle sensors (cameras, traditional millimeter-wave radar): This solution attempts to replace prior maps with real-time perception. Its fundamental flaw lies in the limitations of sensor capabilities. Cameras are susceptible to lighting and weather conditions; traditional millimeter-wave radar has low angular resolution and sparse point clouds, making it unable to stably generate the accurate static road structure representation (such as the boundary and curvature of laneless intersections) required for vehicle motion control. This leads to significant uncertainty in the vehicle's perception of the drivable area, resulting in conservative and erratic trajectory planning and unsmooth control commands.

[0005] The core technical problem: In summary, existing technical solutions, under "mapless" conditions, cannot simultaneously meet the two basic requirements of high-reliability perception and high-smoothness control for autonomous driving at intersections. The fundamental reason lies in the lack of an economical sensor solution that can accurately reconstruct the static geometry of an intersection under all-weather conditions without relying on external prior information. Summary of the Invention

[0006] The purpose of this invention is to overcome the shortcomings of the prior art, especially the core defect that traditional sensors cannot accurately perceive the static road structure in "mapless" scenarios, resulting in unsmooth and uncomfortable vehicle control. This invention provides a smooth control method, system, device, processor, and computer-readable storage medium for mapless intersections based on 4D millimeter-wave radar.

[0007] To achieve the above objectives, the present invention provides a mapless intersection smooth control method, system, device, processor, and computer-readable storage medium based on 4D millimeter-wave radar as follows: The main feature of this mapless intersection smooth control method based on 4D millimeter-wave radar is that the method includes the following steps: (1) Acquire the original point cloud data collected by 4D millimeter-wave radar and preprocess it to obtain the point cloud in the compensated vehicle coordinate system; (2) Adaptive ground segmentation and longitudinal slope modeling are performed on the point cloud to extract the ground point cloud and non-ground point cloud, and a continuous and smooth road surface model is generated. (3) Based on the road surface model and non-ground point cloud, without relying on high-precision prior maps, the static boundary geometric model of the drivable area of ​​the intersection is reconstructed in real time through multi-dimensional feature fusion and geometric model fitting. (4) Based on the static boundary geometry model and the dynamic obstacle information sensed by 4D millimeter-wave radar, a trajectory optimization problem with motion smoothness as the primary optimization objective is constructed, and the optimal control trajectory of the vehicle is obtained by solving the problem. (5) Generate vehicle control commands based on the optimal control trajectory.

[0008] Preferably, step (2) involves adaptive ground segmentation and longitudinal slope modeling, including: The forward sensing area is divided into multiple overlapping longitudinal segments along the vehicle's direction of travel. Local plane fitting is performed on the ground point cloud within each segment; wherein, the ground point cloud is obtained by projecting the point cloud after coordinate compensation onto a polar coordinate grid centered on the vehicle, and using plane fitting based on least squares or RANSAC algorithm to estimate the local ground plane and segment the ground point cloud for the point cloud within each grid. With slope continuity as a constraint, an optimization objective function is constructed, and the planar parameters of all segments are solved jointly to generate a smoothly changing road surface slope model.

[0009] Preferably, step (3) reconstructs the static boundary geometry model in the following manner: Contour extraction is performed on the ground point cloud to generate a first candidate boundary point set; Filter the low vertical object point cloud with a height within a preset threshold range from the non-ground point cloud to generate a second candidate boundary point set; Fuse the first candidate boundary point set and the second candidate boundary point set to generate a fused candidate point set; For each point in the fused candidate point set, perform multi-dimensional feature decision based on at least one of the vertical structure strength, geometric shape characteristics, spatial distribution continuity, and 4D millimeter-wave radar reflection intensity characteristics of its neighborhood point cloud, and screen out high-confidence boundary points; Perform geometric model fitting on the high-confidence boundary points to generate a continuous and differentiable boundary curve.

[0010] Preferably, contour extraction of the ground point cloud includes: Use the Alpha-Shape or rolling ball algorithm to perform contour extraction on the ground point cloud in a two-dimensional plane.

[0011] Preferably, the multi-dimensional feature decision is specifically: Calculate the elevation range ΔZ and elevation standard deviation σ_z of the point cloud in the neighborhood, and judge the vertical structure strength; Perform principal component analysis on the neighborhood point cloud, calculate the linearity L and perpendicularity V, and judge the geometric shape; Calculate the distance between the current point and the nearest neighbor candidate point, and judge the spatial continuity; Calculate the average reflection intensity I_avg of the point cloud in the neighborhood; When ΔZ > Th_Δz and σ_z > Th_σz, and L > Th_L and V < Th_V, and the nearest neighbor distance < Th_d, mark this point as a high-confidence boundary point; Where, Th_Δz is the elevation range threshold, Th_σz is the elevation standard deviation threshold, Th_L is the linearity threshold, Th_V is the perpendicularity threshold, and Th_d is the distance threshold of the nearest neighbor candidate point.

[0012] Preferably, the geometric model fitting is specifically: Receive the macroscopic steering instruction of the navigation system to determine the search area; Perform principal direction analysis on the boundary point set within the search area, and select a piecewise linear model or a B-spline curve model for fitting according to the angle between the principal direction and the current vehicle heading; When there are gaps in the boundary point set, use the endpoint positions, tangent directions, and curvature information of the fitting models at both ends of the gap to construct a Hermite curve or a minimum curvature change curve with continuous curvature for bridging.

[0013] Preferably, the trajectory optimization problem is solved using a quadratic programming framework based on linear time-varying model predictive control, specifically including the following steps: (4.1) Establish the vehicle dynamics model and define the state vector and control input; (4.2) Generate a reference trajectory based on the static boundary geometric model. Use the optimal trajectory or the current reference trajectory obtained in the previous cycle as the linearization point along the reference trajectory in each control cycle to linearize the nonlinear dynamic model and obtain a linear time-varying system. (4.3) The static boundary constraints are linearized into time-varying linear inequalities using local normal vectors; (4.4) Linearize the dynamic obstacle avoidance constraint near the solution of the previous period or the predicted point of the reference trajectory into a time-varying linear inequality; (4.5) Convert the control input, state vector and curvature constraint into box constraints; (4.6) Construct a quadratic objective function with lateral acceleration, longitudinal jerk, curvature tracking error and periodic smoothness as cost terms; (4.7) Integrate the above objectives and constraints into a standard quadratic programming problem, and solve it efficiently through a hot start method to output the optimal control command.

[0014] Preferably, step (4.3) specifically includes: At each discrete time point in the prediction time domain, calculate the normal vector of the left and right boundary curves at the vehicle position; The vehicle profile is simplified to points, and the non-convex constraint "the vehicle profile is within the boundary" is approximated as a linear inequality constraint "the vehicle center lies between two translated parallel lines":

[0015] in, For each discrete time point, For the left boundary curve at the vehicle position The normal vector in the direction of the tangent. For the right boundary curve at the vehicle position The normal vector in the direction of the tangent. This is a constant calculated based on the left boundary curve and the vehicle profile safety margin. This is a constant calculated based on the right boundary curve and the vehicle profile safety margin.

[0016] Preferably, step (4.4) specifically includes: The vehicle and the obstacle at any time Simplify to a circle; The nonlinear collision avoidance constraint is transformed into a time-varying linear inequality constraint by performing a first-order Taylor expansion on the nonlinear collision avoidance constraint near the solution or reference trajectory prediction point of the previous period:

[0017] in, It is a relative position vector. For vehicle location, ( Let be the predicted position of the m-th obstacle at time j. It is a relative position vector; Preferably, step (4.6) specifically includes: (4.6.1) Define the state sequence of the vehicle in the Frenet or Cartesian coordinate system within the planning time domain [t_0, t_0 + T], and represent it using a piecewise polynomial: in, For longitudinal displacement, For horizontal offset, The coefficients are the coefficients of the vertical polynomial. These are the coefficients of the horizontal polynomial; (4.6.2) Construct the quadratic objective function: The first term represents minimizing the lateral acceleration, the second term represents minimizing the longitudinal jerk, and the third term represents minimizing the trajectory curvature. Near-desired curvature The fourth term represents the smooth transition term from the previous cycle trajectory, used to ensure the continuity of control commands. Indicates the weight of lateral acceleration. Indicates the longitudinal acceleration weight. Indicates curvature tracking weights. Indicates the smooth transition weight. Represents the square of the lateral acceleration. Represents the square of longitudinal acceleration. It indicates the degree of difference between adjacent periodic trajectories.

[0018] Preferably, step (4.7) specifically includes: (4.7.1) Gather all objectives and constraints to construct a standard QP form: ; Where z is the optimization variable, H is the objective function matrix, f is a vector, and T is the transpose sign; (4.7.2) The optimal solution obtained using the previous control cycle As the initial iteration point for solving the QP problem in the current cycle, the solver outputs the optimal sequence. Take the first control input This is sent to the vehicle's underlying controller for execution, where u(0) is the control command for the current day, and u_ref(0) is the reference control input for the current time. This represents the optimal control deviation at the current moment.

[0019] Preferably, the method further includes closed-loop adaptive and fault-tolerant steps: Real-time calculation of the smoothness index of the vehicle's trajectory; When the smoothness index is lower than a preset threshold, an anomaly alarm is triggered and the associated static boundary segment is located. A multimodal perception enhancement process is initiated for the aforementioned abnormal boundary segment to generate multiple candidate boundary geometries; A time-domain microprogramming problem is constructed, and forward simulation and cost evaluation are performed on the multiple candidate boundary geometries. The optimal candidate boundary is selected to replace the original boundary. When all candidate boundaries fail to meet the safety threshold, the geometry memory is invoked to retrieve and reuse the geometry of previously successfully passed paths, and the safety degradation control mode is initiated.

[0020] The mapless intersection smoothing control system based on 4D millimeter-wave radar for implementing the above-described method is characterized in that the system comprises: The point cloud perception module is used to acquire 4D millimeter-wave radar point clouds and perform motion compensation, ground segmentation and slope modeling. The static boundary reconstruction module is used to generate a continuous static boundary model of the drivable area of ​​an intersection in real time based on point cloud data. The dynamic target perception module is used to detect, track, and predict the movement trajectory of dynamic obstacles within the intersection; The trajectory optimization and control module is used to construct a trajectory optimization problem with smoothness as the core based on static boundary and dynamic prediction information, and output vehicle control commands. The closed-loop adaptive module is used to monitor control smoothness, trigger the generation and optimal replacement of perception hypotheses when performance degrades, and maintain the geometric memory library.

[0021] Preferably, the static boundary reconstruction module includes: The candidate point generation unit is used to extract candidate boundary points from the ground contour and low vertical objects. Feature decision unit, used for multi-dimensional fusion decision based on vertical structural strength, geometric shape, spatial continuity and reflection intensity; The geometric fitting unit is used for main direction analysis and RANSAC fitting based on steering commands, and supports continuous bridging of breakpoint curvature.

[0022] Preferably, the trajectory optimization and control module adopts the LTV-MPC and QP solution framework, and quickly solves the optimal control command in each control cycle through a hot start method; the closed-loop adaptive module selects the best of different perception hypotheses through a microprogramming forward simulation mechanism, and supports a fallback control strategy based on a historical memory bank.

[0023] The main feature of this mapless intersection smooth control device based on 4D millimeter-wave radar is that the device includes: A processor is configured to execute computer-executable instructions; The memory stores one or more computer-executable instructions, which, when executed by the processor, implement the steps of the above-described smooth control method for mapless intersections based on 4D millimeter-wave radar.

[0024] The main feature of this mapless intersection smooth control processor based on 4D millimeter-wave radar is that the processor is configured to execute computer-executable instructions, which, when executed by the processor, implement the steps of the mapless intersection smooth control method based on 4D millimeter-wave radar described above.

[0025] The computer-readable storage medium is characterized in that it stores a computer program that can be executed by a processor to implement the steps of the above-described smooth control method for mapless intersections based on 4D millimeter-wave radar.

[0026] The present invention employs a mapless intersection smooth control method, system, device, processor, and computer-readable storage medium based on 4D millimeter-wave radar. Without relying on any high-precision prior map or external collaborative facilities, it fully utilizes the imaging capability of high-resolution point clouds of 4D millimeter-wave radar to reconstruct the geometric structure of the passable area of ​​the intersection in real time and accurately, and integrates dynamic target information to ultimately generate an optimized control trajectory that balances safety, comfort (smoothness), and efficiency. This completely solves the technical problem of unsmooth traffic control at intersections under "mapless" conditions. Attached Figure Description

[0027] Figure 1 This is a schematic diagram of the entire closed-loop process from raw data acquisition to control execution of the mapless intersection smooth control method based on 4D millimeter-wave radar of the present invention. Detailed Implementation

[0028] To more clearly describe the technical content of the present invention, the following description is provided in conjunction with specific embodiments.

[0029] Before describing the embodiments of the present invention in detail, it should be noted that, in the following, the terms “comprising,” “including,” or any other variations are intended to cover non-exclusive inclusion, such that a process, method, article, or apparatus that comprises a list of elements includes not only those elements but also other elements not expressly listed or inherent to such process, method, article, or apparatus.

[0030] This technical solution proposes a closed-loop system that does not rely on high-precision prior maps, uses a single or primary 4D millimeter-wave radar as the core sensing source, and combines online structured environment reconstruction with optimal control based on optimization theory. Its core lies in leveraging the long-range, all-weather detection capabilities of 4D millimeter-wave radar and its "quasi-imaging" capability for static, small targets (curbstones, guardrails) to construct a local dynamic map with strong geometric constraints in real time. Within this precise constraint framework, the system solves for a trajectory with "motion comfort (smoothness)" as the primary optimization objective, thereby fundamentally addressing the industry pain points of unsmooth, uncomfortable, and unsafe traffic control at intersections under map-less conditions.

[0031] The implementation of this technical solution will be explained in detail below: I. Refined Mapless Environment Reconstruction Based on 4D Millimeter-Wave Radar The goal of this step is to transform the raw, unordered 4D point cloud P_raw into a structured environment representation that can be understood by a computer and used for precise control.

[0032] (1) First step, point cloud preprocessing and robust ground segmentation: Coordinate unification and motion compensation: The 4D millimeter-wave radar point cloud is transformed from the sensor coordinate system to the vehicle body coordinate system with the rear axle center as the origin. Using the inertial measurement unit (IMU) information provided by the vehicle's CAN bus, the vehicle's own motion (yaw, pitch) within the radar scan cycle is compensated to eliminate point cloud distortion. This is the basis for subsequent accurate segmentation.

[0033] Adaptive ground segmentation: The basic method (meshable plane assumption) projects the compensated point cloud onto a vehicle-centered polar coordinate grid. For the point cloud within each grid, a local ground plane is estimated using least-squares-based plane fitting or the RANSAC algorithm, segmenting the ground point cloud P_ground.

[0034] Innovative Enhancement Method (Longitudinal Segmentation and Slope Adaptation) Crossroads often exhibit longitudinal slope variations (such as uphill turns). The simple global plane assumption becomes ineffective. This technical solution proposes: a. Longitudinal segmentation: Divide the forward sensing area into overlapping segments along the vehicle's forward direction (X-axis) (e.g., each segment is 10 meters long and overlaps by 2 meters).

[0035] b. Slope fitting and continuity constraints within each segment: For the ground point cloud P_ground within each segment S_i, independently fit the ground plane. To eliminate abrupt changes between segments, a slope continuity constraint is introduced, and an optimization problem is constructed to solve for the planar parameters {a_i, b_i, c_i} of all segments.

[0036] Minimize: _i (fitting residual) + _i ( (a_i a_{i 1})^2 + (b_i b_{i 1})^2 ), where For smooth weighting, this method generates a smoothly varying road surface slope model, providing a key benchmark for accurately extracting curb stones located on slopes.

[0037] (2) Second step, refined extraction of static drivable area boundary This section is the core of map-free control. Traditional methods based on fixed elevation thresholds are extremely unreliable on slopes, uneven roads, or when there is vegetation disturbance. This technical solution proposes a dedicated process for multi-stage, multi-feature fusion.

[0038] Phase 1: Multi-source fusion generation of candidate boundary point clouds Generate candidate points from different perspectives to improve recall: Source A (Ground Contour): Perform a 2D (XY plane) Alpha-Shape or rolling ball algorithm on P_ground to extract the geometric contour points C_contour of the ground point cloud.

[0039] Source B (low vertical object): From the non-ground point cloud P_obstacle, select points with height z in [Hmin, Hmax] (e.g., 0.07m to 0.6m) and height above the ground (calculated based on the segmented ground model) within a reasonable range, denoted as C_low.

[0040] Fusion: C_candidate = C_contour ∪ C_low.

[0041] Phase Two: Multi-dimensional Feature Decision Based on Local Context (Core Innovation) For each point p in C_candidate, calculate a set of discriminant features within its spherical neighborhood N(p) with radius R (e.g., 0.4m): the elevation range ΔZ and elevation standard deviation σ_z of the points within N(p) for vertical structural strength calculation.

[0042] Condition 1: ΔZ > Th_Δz (e.g., 0.1m) and σ_z > Th_σz, from which regions with significant vertical changes are selected.

[0043] Principal component analysis (PCA) was performed on N(p) based on geometric morphological features, yielding eigenvalues ​​λ1≥λ2≥λ3.

[0044] Calculate the linearity L = λ1 - λ2 / λ1 and the verticality V = λ3 / λ1. The curbstone dot cloud should be linear and perpendicular to the ground (L high, V low).

[0045] Condition 2: L > Th_L (e.g., 0.65) and V < Th_V (e.g., 0.3).

[0046] Considering the continuity of spatial distribution, calculate the distance d_nn between p and its nearest neighbor in C_candidate.

[0047] Condition 3: d_nn < Th_d (e.g., 0.5m), which ensures that the boundary points are relatively continuous in space.

[0048] Reflection intensity characteristics (unique to 4D radar) Calculate the average reflectance I_avg of N(p). Some curbstone materials (such as metals) have significant reflectance.

[0049] Condition 4 (optional enhancement): I_avg > Th_I or is within a specific range.

[0050] The comprehensive decision logic is as follows: Point p is marked as a high-confidence boundary point p_boundary if and only if (condition 1 AND condition 2 AND condition 3) is satisfied. Condition 4 can serve as the arbitral basis for confidence enhancement when condition 2 is at an edge (such as L being slightly below the threshold). This yields the boundary point set B_set.

[0051] Phase 3: Context-Aware Geometric Model Fitting and Repair Receive macro-level steering instructions (left turn / right turn / straight ahead) from the navigation system.

[0052] Define a fan-shaped search area in front of the vehicle, following the direction indicated by the steering instruction.

[0053] Within this region, a principal direction analysis is performed on the B_set. If the principal direction of the point set distribution makes a small angle with the vehicle's current heading, a piecewise linear model is used; if the angle is large and arc-shaped, a B-spline curve model is used.

[0054] Robust fitting and intelligent bridging are achieved by using Random Sample Consensus (RANSAC) to iteratively fit geometric primitives (lines or B-splines), effectively overcoming interference from misjudged points.

[0055] Key innovation: Disconnection bridging based on kinematic feasibility When there are significant gaps in B_set (e.g., due to occlusion), a simple straight-line connection is not used. Assuming the curvature change of the road boundary is continuous, the fitted models at both ends of the gap are used, with endpoint positions P_0, P_1, tangent directions T_0, T_1, and curvatures κ_0, κ_1. A Hermite curve or minimum curvature change curve satisfying these boundary conditions (C2) is constructed for bridging, ensuring that the generated complete boundary has good smoothness.

[0056] Output: The final output consists of two boundaries, left and right. Each boundary is a continuous, differentiable curve composed of a sequence of geometric primitives, and each primitive is accompanied by a comprehensive confidence score.

[0057] (3) The third step is the perception and prediction of dynamic obstacles. Make full use of the high-precision radial velocity information of 4D millimeter-wave radar to enhance traditional tracking algorithms.

[0058] Improved clustering: For P_obstacle, in addition to using Euclidean distance, radial velocity consistency is introduced as a distance metric to supplement DBSCAN clustering, distance = w1 × spatial distance + w2 × |radial velocity difference|. This can effectively distinguish objects that are spatially close but have different motion states (such as two parallel cars).

[0059] Enhanced target tracking: Employs an interactive multi-model (IMM) filter, with a model set including constant velocity (CV), constant acceleration (CA), and constant rotation (CT) models.

[0060] Key Innovation: 4D Velocity Vector Initialization Traditional trackers exhibit significant uncertainty in the lateral velocity vy when initializing the state vector. This technical solution utilizes the radial velocities of multiple points within the horizontal plane provided by 4D radar, and solves for the initial [vx, vy] using geometric least squares, significantly accelerating tracking convergence and reducing the risk of "ghost targets." Scenario-based trajectory prediction not only employs the CTRA model but also introduces a lightweight intent prediction network. This network takes the target's historical state and its relative position to the reconstructed static boundary of the vehicle as input, and outputs a discrete intent classification (e.g., "go straight along the lane," "enter this lane," "turn at the intersection"). The final predicted trajectory {Traj_pred} is a weighted fusion of the trajectory derived from the physical model and the intent-based sampled trajectory, thus better handling complex interactions at intersections.

[0061] II. Real-time smooth trajectory optimization integrating dynamic and static constraints This step models vehicle motion planning as a convex optimization problem with nonlinear constraints and solves it in real time during each control cycle (e.g., 50ms). The state sequence of the vehicle in the Frenet coordinate system (with the reference centerline as the coordinate axis) or Cartesian coordinate system is defined within the planning time domain [t_0, t_0 + T]. An efficient parameterization method uses piecewise polynomials: in, For longitudinal displacement, For horizontal offset, The coefficients are the coefficients of the vertical polynomial. These are the coefficients of the horizontal polynomial. The optimization variable is the polynomial coefficient vector ξ = [a_0...a_5, b_0...b_5]^T.

[0062] Optimize objective function design (smoothness core): The first item: Minimize lateral acceleration (directly related to ride comfort).

[0063] The second term: Minimize longitudinal jerk (Jerk), this term has a weight. It is usually set to the maximum and is the most critical setting to ensure smoothness.

[0064] Third item: Increase the curvature of the trajectory Close to the desired curvature (This can be calculated from the fitted boundary geometry) to ensure that the trajectory fits the road geometry.

[0065] The fourth item is a smooth transition term with the trajectory of the previous cycle, ensuring the continuity of control commands.

[0066] in, Indicates the weight of lateral acceleration. Indicates the longitudinal acceleration weight. Indicates curvature tracking weights. Indicates the smooth transition weight. Represents the square of the lateral acceleration. Represents the square of longitudinal acceleration. It indicates the degree of difference between adjacent periodic trajectories.

[0067] Constraint Construction: Static corridor constraint (hard constraint): The left and right boundary models B_left(s) and B_right(s) obtained above define a time-varying feasible corridor. The constraint is: Dynamic obstacle avoidance constraint (time-varying constraint): The vehicle and obstacles are simplified to circumscribed circles or rectangles. For each predicted time t, the center distance between the vehicle and each dynamic obstacle k is constrained to be greater than the safe radius R_safe. Linearization technique: To facilitate the solution, the above nonlinear constraints can be transformed into linear constraints by performing a first-order Taylor expansion near the solution of the previous period.

[0068] Vehicle dynamics constraints: constrain the velocity v(t), acceleration a(t), and curvature κ(t) within the vehicle's physical limits.

[0069] Building upon this foundation, this technical solution proposes an efficient trajectory optimization implementation scheme: a quadratic programming (QP) solution framework based on Linear Time-Varying MPC (LTV-MPC). The core innovation of this scheme lies in designing a complete linearization, discretization, and convex approximation process for time-varying, non-convex static boundary constraints and dynamic obstacle avoidance constraints in mapless intersection scenarios. This transforms the problem into a standard QP problem, thereby meeting the stringent real-time requirements (<50ms) of the onboard platform. The specific implementation is as follows: Step 1: Vehicle Dynamics Model and State Space Definition The kinematic bicycle model, which balances accuracy and computational efficiency, was chosen as the prediction model. The state vector is defined as follows: , representing the lateral position, longitudinal position, heading angle, and velocity of the vehicle's rear axle center in the vehicle coordinate system, respectively. The control input is u = [a, δ]^T, which is the acceleration and the front wheel steering angle.

[0070] Continuous-time nonlinear model: ; ; ; V = a; Where L is the vehicle wheelbase, this model is non-linear.

[0071] Step 2: Reference Trajectory Generation and Model Linearization (Key Innovative Process) The reference trajectory generation does not rely on a fixed path, but rather generates a rough reference centerline ξ_ref(t) in real time based on the static boundary corridor reconstructed by 4D radar. For example, the centerline of the left and right boundaries can be taken, or a simple polynomial curve that satisfies the boundary constraints can be used.

[0072] Linearization along the reference trajectory is performed in each control cycle k, using the optimal trajectory solved in the previous cycle or the current reference trajectory as the linearization point. For the nonlinear dynamic model... Performing a first-order Taylor expansion at this point yields a linear time-varying (LTV) system: (t) = A(t)× (t) + B(t)× u(t) in, Let k be the reference state at time k. For the reference control at time k, = _ref, u = u u_ref, A(t) and B(t) are time-varying Jacobian matrices. This is a key step in transforming a nonlinear problem into a convex optimization problem.

[0073] Step 3: Convex Approximation and Discretization of Constraints This is the core of embedding complex, graph-free environments into optimization problems. Static boundary constraints (time-varying, non-convex → time-varying, linear) The reconstructed left and right boundaries in this technical solution are parameterized curves. At each discrete time point j in the prediction time domain, the vehicle profile is simplified into a curve with... The center point. Calculate the left and right boundary curves B_left(s) and B_right(s) at that point. The normal vectors n_left(j) and n_right(j) of the nearby tangent direction.

[0074] The non-convex constraint "vehicle profile within the boundary" is conservatively approximated as "vehicle center point lies between two translated parallel lines," forming a linear inequality constraint: in, , It is a constant calculated based on the boundary curve and the vehicle profile safety margin. This linearization method using local normal vectors is the innovative aspect of this technical solution for efficiently handling complex geometric constraints.

[0075] Dynamic obstacle avoidance constraints (nonlinear → linear): The vehicle and obstacle m at time j are simplified to circles (with radii R_ego and R_obs_m, respectively).

[0076] Collision avoidance constraints It is non-linear.

[0077] Key processing: Predicting points from the previous cycle's solution or reference trajectory. First-order linearization is performed on the vicinity. Define the relative position vector r_ref = pos_ref - pos_obs_ref, then the linearized constraint approximates as: in, It is a relative position vector. For vehicle location, ( Let be the predicted position of the m-th obstacle at time j. It is a relative position vector; This becomes a time-varying linear inequality constraint. To ensure safety, constraint tightening or robust tube terms can be added to compensate for linearization errors and prediction uncertainties.

[0078] Control and State Constraints: The upper and lower limits of acceleration a, front wheel steering angle δ, and velocity v are natural box constraints, which are already linear.

[0079] The system is discretized using zero-order hold (ZOH) or first-order Euler method, discretizing the continuous LTV system and all constraints in the prediction time domain N to obtain a discrete-time linear system: (k+1) = A_k× (k) + B_k× u(k), for k = 0, ... , N-1 Step 4: Formulating the Quadratic Programming (QP) Problem By bringing together all objectives and constraints, a standard QP (Quality of Practice) format can be constructed: .

[0080] Where the optimization variable z is the state-input deviation sequence in the prediction time domain[ (0), u(0), ..., (N-1), u(N-1), (N)].

[0081] Construction of the objective function matrix H and vector f: a smoothness objective, corresponding to minimizing acceleration and jerk. For example, minimizing the control input. u (especially) rate of change Δ The quadratic terms of ) can be directly reflected in the H matrix.

[0082] Track the target and minimize state deviation. (Especially the quadratic terms of position and heading deviations).

[0083] Terminal costs, to enhance stability, include monitoring terminal status. (N) is assigned a higher weight.

[0084] The various weighting coefficients Q and R have been carefully adjusted. Among them, the penalty for jerk can be achieved by penalizing Δδ and Δa, which is the core of ensuring comfort.

[0085] Construction of constraint matrices A_ineq and A_eq: Constructing equality constraints All linearized constraints, static boundary constraints, dynamic obstacle avoidance constraints, and control and state constraints together constitute linear inequality constraints. These constraint matrices are time-varying and sparse.

[0086] The following section will explain in detail the processing of control input, state vector, and curvature constraints into box constraints in this technical solution: (1) Optimize variable definition In the LTV-MPC framework, the optimization variables are the sequence of state deviations and control input deviations within the prediction time domain N: Among them, state variables Deviation Control input Deviation , For longitudinal acceleration, This refers to the angle of the front wheels.

[0087] (2) Transformation of box-type constraints for control input Step 1: Extract the physical limits Obtain actuator hard constraints from vehicle parameters: (Acceleration limits, such as -5 to 3 m / s²) (Steering angle mechanical limits, such as -35°~35°) Step 2: Convert into constraints on deviation. because The above constraints are transformed into: Step 3: Write the orientation constraint matrix For each prediction step set up: in: (3) Transformation of state vector into box constraint Step 1: Velocity Constraint Road speed limits: Transform into deviation constraints: Step 2: Heading Angle Constraint If it is necessary to limit the search range by heading angle: Usually taken , Step 3: Write the box constraint matrix For each prediction step ,set up: in: Positions x and y are usually not subject to box constraints, but are limited by other constraints.

[0088] (4) Curvature constraint is transformed into Box-shaped constraints Step 1: Extract curvature information from static boundary Calculate the curvature k_ref(s) of the reference centerline from the reconstructed left and right boundaries B_left(s) and B_right(s): Step 2: Calculation of safe vehicle speed based on curvature Based on the maximum permissible lateral acceleration a_y_max (e.g.) ) and the curvature at the current point, calculate the safe vehicle speed: Step 3: Dynamic steering angle limiting Using bicycle models (L is the wheelbase), transforming curvature constraints into... Constraints: Basic curvature constraint: | in, This represents the maximum curvature of the road segment.

[0089] Velocity adaptive constraints: Considering lateral acceleration limitations, the upper limit of the speed-dependent steering angle is derived as follows: Step 4: Form time-varying box-shaped constraints For each prediction step k, the dynamic steering angle limit is calculated based on the reference velocity v_ref(k) and curvature k_ref(k) at that point: This translates into a constraint on the control input deviation: Step 5: Update control input box-type constraints Replace the static steering angle constraint in step two with the dynamic constraint described above: The above completes the conversion of box-type constraints.

[0090] Step 5: Efficient Solution and Warm Startup The solver chosen is a QP solver specifically optimized for embedded systems, such as OSQP (based on the ADMM algorithm). This efficiently handles the sparse, time-varying constraint matrix of this problem. The optimal solution obtained in the previous control cycle is utilized. This serves as the initial iteration point for solving the QP problem in the current cycle. Since the problems in adjacent cycles are similar, this significantly reduces the number of solver iterations, a key technology for achieving real-time performance of <50ms. The solver outputs the optimal sequence. Take the first control input It is then sent to the vehicle's underlying controller for execution.

[0091] Step Six: Robustness Enhancement and Fault Handling When a QP problem becomes unsolvable due to overly tight constraints, this technical solution introduces soft constraints or constraint relaxation. For example, dynamic obstacle avoidance constraints are transformed into penalty terms added to the objective function, allowing performance degradation to be traded for system safety.

[0092] When the confidence of static boundary constraints is low, their weights as slack variables in QP problems are automatically increased, or their corresponding safety distances d_left / right(j) are relaxed, making the optimizer more inclined to rely on information with high confidence (such as dynamic constraints) for decision-making, which reflects system-level adaptation.

[0093] III. Closed-loop adaptive and fault-tolerant mechanisms Traditional autonomous driving systems typically employ a one-way open-loop perception and planning module, with the planning module passively receiving perception results. One of the core innovations of this technical solution lies in constructing a closed-loop feedback system with planning and control performance (smoothness) as its optimization objective. When control quality deteriorates, the system can perform reverse diagnostics and guide the perception module to make targeted online adjustments, thereby achieving self-optimization in dynamic environments.

[0094] First, smoothness monitoring and anomaly tracing.

[0095] Real-time smoothness metric calculation: In each control cycle (e.g., 50ms), calculate the jerk_actual and rate of curvature change of the actual executed trajectory. Define a smoothness metric S_score: in, For smoothness scoring, The first weighting coefficient, The first attenuation coefficient, The square root of the accelerometer. This is the weighting coefficient for the second term. This is the second attenuation coefficient. This represents the absolute value of the maximum rate of change of curvature.

[0096] Anomaly detection and correlation analysis: When S_score exceeds the preset comfort threshold S_th for N consecutive cycles (e.g., 5 cycles, corresponding to 250ms), an anomaly alarm is triggered. The system immediately locks the spatiotemporal window that causes high Jerk or high curvature changes and backtracks to the static boundary constraint segment used by the planner during that time period.

[0097] Secondly, a confidence-driven multimodal perception enhancement process.

[0098] Once it is determined that the anomaly is related to a certain static boundary B_seg, the system initiates the following multi-level adaptive process, rather than simply adjusting parameters: Level 1: Boundary geometric confidence reassessment and data re-acquisition Extracting the original perception context: The system retrieves the original 4D radar point cloud snapshot and corresponding ground segmentation parameters used to generate B_seg during the abnormal period.

[0099] Assume H1 (relaxed segmentation): temporarily relax the height threshold Δz_ground of ground segmentation by Δ% (e.g., 50%), and re-segment to obtain a point cloud set P_candidate_new containing more "suspected curbstone" low points.

[0100] Assumption H2 (Adjusted Feature Decision): In the boundary point decision maker (see “Refined Extraction of Static Driving Area Boundaries” above), temporarily reduce the threshold Th_L of geometric features (linearity L) and increase the weight of vertical structure strength (σ_z) to capture weaker or more irregular boundary features.

[0101] Assumption H3 (Alternate Model Enabled): Bypassing the PCA-based decision maker, a lightweight one-dimensional convolutional neural network (1D-CNN) is applied directly to C_candidate. This CNN is trained offline to directly identify curb patterns from a sequence of one-dimensional height profiles of the point cloud, serving as a backup when traditional geometric methods fail.

[0102] Generate multiple hypothesis boundaries: Based on three strategies, H1, H2, and H3, generate three candidate boundary geometries B_seg_h1, B_seg_h2, and B_seg_h3 in parallel.

[0103] Level Two: Forward Simulation and Optimal Hypothesis Selection This is the core decision-making logic of the feedback mechanism, replacing the simple "choose the smoothest solution".

[0104] Constructing a microprogramming problem: Starting with the vehicle's current state, create a simplified trajectory optimization problem with a shortened time domain (e.g., 1 second). This problem inherits all dynamic constraints and objective functions from the main planner, but only for the problematic B_seg region, substitutes three candidate boundaries B_seg_hX as static constraints.

[0105] Parallel forward simulation and performance evaluation: Fast solution (or sampling evaluation) of three microprogramming problems to obtain three candidate trajectories. _h1, _h2, _h3. Calculate the comprehensive cost C_cost for each trajectory: C_cost = ×Jerk_RMS + ×Curvature Cost+ × Deviation from historical trajectory + ×(1 / Original point cloud density of the boundary assumption) This cost function balances smoothness, consistency, and perceived strength of evidence.

[0106] Dynamic selection and model update: The candidate boundary hypothesis B_seg_best with the minimum C_cost is selected. If its cost is significantly lower than that of the original boundary B_seg, the system dynamically replaces the boundary output of the corresponding region in the main perception thread with B_seg_best and updates the confidence of that boundary segment. Simultaneously, special annotation of the point cloud data in that region is triggered for subsequent offline analysis to optimize model parameters.

[0107] Level 3: Robust fallback strategy based on spatiotemporal memory When all hypotheses at level two fail to generate trajectories that meet the safety threshold (e.g., C_cost is still too high), it indicates that the perception in that area is extremely unreliable, and the system activates a fallback strategy: Short-term memory retrieval: The system accesses a maintained "geometric memory bank." This bank stores the geometry of paths recently successfully and smoothly traversed by the vehicle in a local coordinate system. By matching the current vehicle pose with the entry point in the memory bank, a "confirmed" boundary corridor corresponding to a historical trajectories is retrieved and reused. The reused boundary is marked as a "memory guide."

[0108] Safety Degradation Control: While activating memory guidance, the system forces the vehicle into a "Cautionary Passage" mode: reducing the planned speed limit, increasing the safe distance from dynamic obstacles, and issuing prompts via the human-machine interface. This mode continues until the vehicle leaves the low-confidence area and the smoothness index returns to normal.

[0109] Memory learning and updating: After successfully traversing in the "Cautious Passage" mode, the system will use the actual driving trajectory (smoothed by post-processing) and its corresponding perception data (although with low confidence) as a new positive example and encrypt and store it in the "Geometric Memory Bank" to enrich the experience of dealing with similar scenarios in the future.

[0110] The structural innovation of closed-loop feedback: using the high-level control performance index (smoothness) as the feedback signal to drive the parameters and model selection of the low-level perception algorithm, breaks down the barriers between traditional modules.

[0111] A specific online multi-hypothesis evaluation framework is proposed: a specific multi-hypothesis generation method including "relaxed segmentation - adjusted decision - backup model" is presented, along with a specific decision-making logic for forward simulation optimization by constructing and solving a microprogramming problem. This goes beyond simply adjusting thresholds or rules.

[0112] Introducing a spatiotemporal memory bank with learning capabilities: "Historical information reuse" is concretized into a searchable and updatable "geometric memory bank", and a security degradation control mode linked to it is designed, forming a complete fault response and experience learning subsystem.

[0113] This technical solution addresses the inherent deficiencies in perception accuracy and control smoothness of existing "mapless" autonomous driving solutions for intersections through systematic innovation, resulting in the following significant beneficial effects: Technical effect 1: It achieves highly reliable, all-weather, accurate perception of the static environment of intersections, significantly improving the safety boundary of the mapless system.

[0114] Reason for the improvement: This technical solution creatively changes the core perception from traditional multi-sensor fusion to primarily using 4D millimeter-wave radar, and designs a static boundary extraction algorithm specifically for its point cloud characteristics. Because 4D millimeter-wave radar possesses high-resolution detection capabilities for low-lying stationary targets such as curbs and guardrails, and is unaffected by weather conditions such as sunlight, rain, and fog, the improved perception system can stably and in real-time reconstruct a drivable area boundary model containing precise geometric information even without a prior map. This provides a reliable "virtual track" for vehicle planning, fundamentally avoiding the risk of driving off the road or colliding with static objects due to unclear perception, thus ensuring basic traffic safety under any weather conditions.

[0115] Technical effect 2: It achieves extremely smooth control in complex intersection scenarios, greatly improving ride comfort and the degree of humanization of driving.

[0116] Cause of the problem: In this technical solution, the trajectory planning problem is constructed as a constrained optimization problem with kinematic smoothness as the primary optimization objective, and the weight of the jerk minimization term is particularly strengthened. Because the improved optimization model uses precise static boundaries as hard constraints and dynamic obstacle avoidance and vehicle dynamics as constraints, the solved trajectory is not only mathematically smooth (continuous higher-order derivatives) but also physically feasible. This makes the vehicle's acceleration, deceleration, and steering actions during turning and obstacle avoidance continuous, smooth, and without jerks, resulting in a comfortable riding experience "like that of an experienced driver," an effect that traditional rule-based or search-based planning methods cannot achieve.

[0117] Thirdly, it reduces the system's reliance on expensive sensors and high-cost infrastructure, providing a more commercially viable autonomous driving solution.

[0118] Reason for its success: This technical solution achieves core functionality by replacing the expensive sensor combination (LiDAR + high-precision map) with a single type of economical sensor (4D millimeter-wave radar) through algorithmic innovation. Since the cost of 4D millimeter-wave radar is far lower than that of LiDAR, and this solution eliminates the need for building and maintaining V2X roadside facilities or high-precision maps covering the entire area, the overall hardware and data costs of the improved system are significantly reduced. This makes it possible to popularize advanced autonomous driving functions in mass-produced vehicles, thus creating a universally beneficial commercial effect of being "affordable and easy to use."

[0119] Technical benefit 4: It endows the system with strong self-adaptation and fault tolerance capabilities, and improves the system's robustness in non-standard scenarios and extreme conditions.

[0120] Cause: This technical solution introduces a perception-control closed-loop feedback mechanism based on a smoothness index. When the control module detects a decrease in trajectory smoothness, it provides confidence feedback to the perception module. Because this mechanism can trigger adaptive parameter adjustment or multi-hypothesis fusion of the perception algorithm, the system can proactively optimize its environmental understanding and prioritize the stability of the control output when facing challenges such as non-standard intersection geometry, temporary construction barriers, or temporary degradation in point cloud quality. This makes the system no longer a fragile open-loop chain, resulting in a robust effect that becomes more stable with use and can cope with long-tail scenarios.

[0121] Any process or method description in the flowchart or otherwise herein can be understood as representing a module, segment, or portion of code comprising one or more executable instructions for implementing a particular logical function or process, and the scope of the preferred embodiments of the invention includes additional implementations in which functions may be performed not in the order shown or discussed, including substantially simultaneously or in reverse order depending on the functions involved, as will be understood by those skilled in the art to which embodiments of the invention pertain.

[0122] It should be understood that various parts of the present invention can be implemented using hardware, software, firmware, or a combination thereof. In the above embodiments, multiple steps or methods can be implemented using software or firmware stored in memory and executed by a suitable instruction execution device.

[0123] Those skilled in the art will understand that all or part of the steps of the methods in the above embodiments can be implemented by a program instructing related hardware. The program can be stored in a computer-readable storage medium, and when executed, the program includes one or a combination of the steps of the method embodiments.

[0124] The storage media mentioned above can be read-only memory, disk, or optical disk, etc.

[0125] In the description of this specification, references to terms such as "an embodiment," "some embodiments," "example," "specific example," or "embodiment," etc., indicate that a specific feature, structure, material, or characteristic described in connection with that embodiment or example is included in at least one embodiment or example of the invention. In this specification, illustrative expressions of the above terms do not necessarily refer to the same embodiment or example. Furthermore, the specific features, structures, materials, or characteristics described may be combined in any suitable manner in one or more embodiments or examples.

[0126] Although embodiments of the present invention have been shown and described above, it is understood that the above embodiments are exemplary and should not be construed as limiting the present invention. Those skilled in the art can make changes, modifications, substitutions and variations to the above embodiments within the scope of the present invention.

[0127] In this specification, the invention has been described with reference to specific embodiments thereof. However, it will be apparent that various modifications and variations can be made without departing from the spirit and scope of the invention. Therefore, the specification and drawings should be considered illustrative rather than restrictive.

Claims

1. A mapless intersection smooth control method based on 4D millimeter-wave radar, characterized in that, The method includes the following steps: (1) Acquire the original point cloud data collected by 4D millimeter-wave radar and preprocess it to obtain the point cloud in the compensated vehicle coordinate system; (2) Adaptive ground segmentation and longitudinal slope modeling are performed on the point cloud to extract the ground point cloud and non-ground point cloud, and a continuous and smooth road surface model is generated. (3) Based on the road surface model and non-ground point cloud, without relying on high-precision prior maps, the static boundary geometric model of the drivable area of ​​the intersection is reconstructed in real time through multi-dimensional feature fusion and geometric model fitting. (4) Based on the static boundary geometry model and the dynamic obstacle information sensed by 4D millimeter-wave radar, a trajectory optimization problem with motion smoothness as the primary optimization objective is constructed, and the optimal control trajectory of the vehicle is obtained by solving the problem. (5) Generate vehicle control commands based on the optimal control trajectory.

2. The smooth control method for mapless intersections based on 4D millimeter-wave radar according to claim 1, characterized in that, Step (2) involves adaptive ground segmentation and longitudinal slope modeling, including: The forward sensing area is divided into multiple overlapping longitudinal segments along the vehicle's direction of travel. Local plane fitting is performed on the ground point cloud within each segment; wherein, the ground point cloud is obtained by projecting the point cloud after coordinate compensation onto a polar coordinate grid centered on the vehicle, and using plane fitting based on least squares or RANSAC algorithm to estimate the local ground plane and segment the ground point cloud for the point cloud within each grid. With slope continuity as a constraint, an optimization objective function is constructed, and the planar parameters of all segments are solved jointly to generate a smoothly changing road surface slope model.

3. The smooth control method for mapless intersections based on 4D millimeter-wave radar according to claim 1, characterized in that, Step (3) reconstructs the static boundary geometry model in the following manner: Contour extraction is performed on the ground point cloud to generate a first candidate boundary point set; From the non-ground point cloud, select low-lying vertical object point clouds whose height is within a preset threshold range to generate a second candidate boundary point set; The first candidate boundary point set and the second candidate boundary point set are merged to generate a fused candidate point set; For each point in the fusion candidate point set, multi-dimensional feature judgment is performed based on at least one of the vertical structure strength, geometric morphology features, spatial distribution continuity and 4D millimeter-wave radar reflection intensity features of its neighborhood point cloud to screen out high-confidence boundary points. Geometric model fitting is performed on the high-confidence boundary points to generate continuous, differentiable boundary curves.

4. The smooth control method for mapless intersections based on 4D millimeter-wave radar according to claim 3, characterized in that, Contour extraction of the ground point cloud includes: The Alpha-Shape or Rolling Ball algorithm is used to extract the contours of the ground point cloud in a two-dimensional plane.

5. The smooth control method for mapless intersections based on 4D millimeter-wave radar according to claim 3, characterized in that, The multidimensional feature decision is specifically as follows: Calculate the elevation range ΔZ and elevation standard deviation σ_z of the point cloud within the neighborhood to determine the strength of the vertical structure; Principal component analysis is performed on the neighborhood point cloud to calculate linearity L and verticality V, and to determine the geometric shape. Calculate the distance between the current point and its nearest neighbor candidate points to determine spatial continuity; Calculate the average reflection intensity I_avg of the point cloud within the neighborhood; When ΔZ > Th_Δz and σ_z > Th_σz, and L > Th_L and V < Th_V, and the nearest neighbor distance < Th_d, mark the point as a high-confidence boundary point; Where Th_Δz is the elevation range threshold, Th_σz is the elevation standard deviation threshold, Th_L is the linearity threshold, Th_V is the verticality threshold, and Th_d is the distance threshold to the nearest neighbor candidate point.

6. The smooth control method for mapless intersections based on 4D millimeter-wave radar according to claim 3, characterized in that, The geometric model fitting specifically refers to: Receive macro-level steering instructions from the navigation system and determine the search area; Perform principal direction analysis on the set of boundary points within the search area, and select a piecewise linear model or a B-spline curve model for fitting based on the angle between the principal direction and the vehicle's current heading. When gaps exist in the boundary point set, the endpoint positions, tangent directions, and curvature information of the fitted models at both ends of the gap are used to construct a Hermitian curve with continuous curvature or a minimum curvature change curve for bridging.

7. The smooth control method for mapless intersections based on 4D millimeter-wave radar according to claim 1, characterized in that, The trajectory optimization problem is solved using a quadratic programming framework based on linear time-varying model predictive control, specifically including the following steps: (4.1) Establish the vehicle dynamics model and define the state vector and control input; (4.2) Generate a reference trajectory based on the static boundary geometric model. Use the optimal trajectory or the current reference trajectory obtained in the previous cycle as the linearization point along the reference trajectory in each control cycle to linearize the nonlinear dynamic model and obtain a linear time-varying system. (4.3) The static boundary constraints are linearized into time-varying linear inequalities through local normal vectors; (4.4) Linearize the dynamic obstacle avoidance constraint near the solution of the previous period or the predicted point of the reference trajectory into a time-varying linear inequality; (4.5) Convert the control input, state vector and curvature constraint into box constraints; (4.6) Construct a quadratic objective function with lateral acceleration, longitudinal jerk, curvature tracking error and periodic smoothness as cost terms; (4.7) Integrate the above objectives and constraints into a standard quadratic programming problem, and solve it efficiently through a hot start method to output the optimal control command.

8. The smooth control method for mapless intersections based on 4D millimeter-wave radar according to claim 7, characterized in that, The specific steps (4.3) are as follows: At each discrete time point in the prediction time domain, calculate the normal vector of the left and right boundary curves at the vehicle position; The vehicle profile is simplified to points, and the non-convex constraint "the vehicle profile is within the boundary" is approximated as a linear inequality constraint "the vehicle center lies between two translated parallel lines": in, For each discrete time point, For the left boundary curve at the vehicle position The normal vector in the direction of the tangent. For the right boundary curve at the vehicle position The normal vector in the direction of the tangent. This is a constant calculated based on the left boundary curve and the vehicle profile safety margin. This is a constant calculated based on the right boundary curve and the vehicle profile safety margin.

9. The smooth control method for mapless intersections based on 4D millimeter-wave radar according to claim 7, characterized in that, The specific steps (4.4) are as follows: The vehicle and the obstacle at any time Simplify to a circle; The nonlinear collision avoidance constraint is transformed into a time-varying linear inequality constraint by performing a first-order Taylor expansion on the nonlinear collision avoidance constraint near the solution or reference trajectory prediction point of the previous period: in, It is a relative position vector. For vehicle location, ( Let be the predicted position of the m-th obstacle at time j. This is a relative position vector.

10. The smooth control method for mapless intersections based on 4D millimeter-wave radar according to claim 7, characterized in that, The specific steps (4.6) are as follows: (4.6.1) Define the state sequence of the vehicle in the Frenet or Cartesian coordinate system within the planning time domain [t_0, t_0 + T], and represent it using a piecewise polynomial: in, For longitudinal displacement, For horizontal offset, The coefficients are the coefficients of the vertical polynomial. These are the coefficients of the horizontal polynomial; (4.6.2) Construct the quadratic objective function: The first term represents minimizing the lateral acceleration, the second term represents minimizing the longitudinal jerk, and the third term represents minimizing the trajectory curvature. Near-desired curvature The fourth term represents the smooth transition term from the previous cycle trajectory, used to ensure the continuity of control commands. Indicates the weight of lateral acceleration. Indicates the longitudinal acceleration weight. Indicates curvature tracking weights. Indicates the smooth transition weight. Represents the square of the lateral acceleration. Represents the square of longitudinal acceleration. It indicates the degree of difference between adjacent periodic trajectories.

11. The mapless intersection smooth control method based on 4D millimeter-wave radar according to claim 10, characterized in that, The aforementioned step (4.7) specifically includes: (4.7.1) Gather all objectives and constraints to construct a standard QP form: ； Where z is the optimization variable, H is the objective function matrix, f is a vector, and T is the transpose sign; (4.7.2) The optimal solution obtained using the previous control cycle As the initial iteration point for solving the QP problem in the current cycle, the solver outputs the optimal sequence. Take the first control input This is sent to the vehicle's underlying controller for execution, where u(0) is the control command for the current day, and u_ref(0) is the reference control input for the current time. This represents the optimal control deviation at the current moment.

12. The mapless intersection smooth control method based on 4D millimeter-wave radar according to claim 1, characterized in that, The method also includes closed-loop adaptive and fault-tolerant steps: Calculate the smoothness index of the vehicle's trajectory in real time; When the smoothness index is lower than a preset threshold, an anomaly alarm is triggered and the associated static boundary segment is located. A multimodal perception enhancement process is initiated for the aforementioned abnormal boundary segment to generate multiple candidate boundary geometries; A time-domain microprogramming problem is constructed, and forward simulation and cost evaluation are performed on the multiple candidate boundary geometries. The optimal candidate boundary is selected to replace the original boundary. When all candidate boundaries fail to meet the safety threshold, the geometry memory is invoked to retrieve and reuse the geometry of previously successfully passed paths, and the safety degradation control mode is initiated.

13. A mapless intersection smoothing control system based on 4D millimeter-wave radar for implementing the method of any one of claims 1 to 12, characterized in that, The system includes: The point cloud perception module is used to acquire 4D millimeter-wave radar point clouds and perform motion compensation, ground segmentation and slope modeling. The static boundary reconstruction module is used to generate a continuous static boundary model of the drivable area of ​​an intersection in real time based on point cloud data. The dynamic target perception module is used to detect, track, and predict the movement trajectory of dynamic obstacles within the intersection; The trajectory optimization and control module is used to construct a trajectory optimization problem with smoothness as the core based on static boundary and dynamic prediction information, and output vehicle control commands. The closed-loop adaptive module is used to monitor control smoothness, trigger the generation and optimal replacement of perception hypotheses when performance degrades, and maintain the geometric memory library.

14. The mapless intersection smooth control system based on 4D millimeter-wave radar according to claim 13, characterized in that, The static boundary reconstruction module includes: The candidate point generation unit is used to extract candidate boundary points from the ground contour and low vertical objects. Feature decision unit, used for multi-dimensional fusion decision based on vertical structural strength, geometric shape, spatial continuity and reflection intensity; The geometric fitting unit is used for main direction analysis and RANSAC fitting based on steering commands, and supports continuous bridging of breakpoint curvature.

15. The mapless intersection smooth control system based on 4D millimeter-wave radar according to claim 13, characterized in that, The trajectory optimization and control module adopts the LTV-MPC and QP solution framework and quickly solves the optimal control command in each control cycle through a hot start method; the closed-loop adaptive module selects the best of different perception hypotheses through a microprogramming forward simulation mechanism and supports a fallback control strategy based on a historical memory bank.

16. A mapless intersection smooth control device based on 4D millimeter-wave radar, characterized in that, The device includes: A processor is configured to execute computer-executable instructions; The memory stores one or more computer-executable instructions, which, when executed by the processor, implement the steps of the mapless intersection smooth control method based on 4D millimeter-wave radar as described in any one of claims 1 to 12.

17. A mapless intersection smooth control processor based on 4D millimeter-wave radar, characterized in that, The processor is configured to execute computer-executable instructions, which, when executed by the processor, implement the steps of the mapless intersection smooth control method based on 4D millimeter-wave radar as described in any one of claims 1 to 12.

18. A computer-readable storage medium, characterized in that, It stores a computer program that can be executed by a processor to implement the steps of the mapless intersection smooth control method based on 4D millimeter-wave radar as described in any one of claims 1 to 12.

Images