Automatic parking path planning and tracking control method for narrow environment

Abstract

The application discloses a kind of automatic parking path planning and tracking control method for narrow environment, belong to automatic driving intelligent path planning and control technical field.The application first constructs two kinds of parking scene of parallel and vertical, designs parking stall evaluation function to score current parking stall, selects the highest parking stall of score parking.The second improvement cost function of hybrid algorithm, join smoothness cost, safety distance cost and the punishment to frequent switching vehicle input control, can guarantee the safety of parking path while improving the smoothness of path, design model predictive control tracking controller, and through particle swarm optimization prediction time domain and control time domain, while reducing the amount of calculation of guaranteeing tracking accuracy.Carsim and Matlab / Simulink joint simulation platform verification, the parking path length planned by the application is short, quality is high, and does not need to frequently change direction, path tracking effect error is small, can satisfy the parking requirement under narrow environment.

Description

Technical Field

[0001] This invention belongs to the field of intelligent path planning and control technology for autonomous driving, and specifically relates to an automatic parking path planning and tracking control method for narrow environments. Background Technology

[0002] In recent years, with the rapid development of computer technology and perception technology, autonomous driving technology is moving from partial to full autonomous driving. Automated parking systems, as a key component of autonomous driving technology, are now widely used in various parking scenarios. However, when faced with narrow parking spaces, current automated parking technologies suffer from problems such as difficulty in single-step parking, discontinuous path curvature, and low path tracking accuracy. How to safely and efficiently complete parking tasks in narrow environments has become a key focus of current automated parking technology. To enable autonomous vehicles to complete parking tasks more safely and efficiently in narrow environments, it is necessary to plan parking trajectories that meet feasibility, safety, and comfort requirements.

[0003] Hybrid A* and Model Predictive Control (MPC) have been widely applied in trajectory planning for autonomous vehicles. MPC predicts the vehicle's state over a finite time domain based on an existing model and the current state. It then designs an appropriate objective function and constraints to construct an optimization problem, solving which yields the optimal control variables for planning and control. However, complex models increase the computational cost of MPC, significantly impacting the real-time performance of autonomous vehicle planning and control. Hybrid A*, a classic path planning method, incorporates vehicle kinematic constraints into the A* algorithm. It uses continuous poses as search nodes and adheres to Ackerman steering and minimum turning radius constraints, meeting the executability requirements of nonholonomically constrained vehicles. Furthermore, it introduces heuristics such as discrete grid management and Reeds-Sheep to balance search efficiency and global optimality. It also supports forward and reverse direction changes, meeting requirements such as narrow structured parking spaces, high posture requirements, and the need for multi-segment driving. However, the paths searched by Hybrid A* often suffer from insufficient smoothness and are prone to failure in extremely narrow conditions, which means that the feasibility and safety of autonomous vehicles parking in narrow environments need to be improved. Summary of the Invention

[0004] The technical problem to be solved by this invention is to overcome the problems of difficulty in single-step parking, discontinuous path curvature, and low path tracking accuracy in narrow parking environments. It proposes an automatic parking path planning and tracking control method for narrow environments, which obtains a smoother and more efficient parking path while meeting the requirements of feasibility and safety.

[0005] This invention provides an automatic parking path planning and tracking control method for confined environments. The system involved in this method includes a behavior decision module, a parking path planning module, and a trajectory tracking module. The method includes the following steps:

[0006] Step 1: Environmental Modeling and Parking Space Detection

[0007] The problem of automatic parking path planning in narrow environments is transformed into a multi-constraint optimization problem. The parking path consists of a starting point, a target parking space, and several nodes to be optimized. The feasible solution space is constrained by the parking area boundaries and obstacles. Onboard sensors (such as lidar and ultrasonic radar) are used to perceive the environment, search for available parking spaces, and determine the parking mode based on the length-to-width ratio of the parking space. The parking modes include parallel parking and perpendicular parking. A parallel parking mode is defined as a space whose length-to-width ratio is greater than 1, and a perpendicular parking mode is defined as a space whose length-to-width ratio is less than 1.

[0008] Step 2: Select the best available parking space

[0009] When multiple vacant parking spaces are detected, a hybrid A* algorithm is used to perform a coarse path search for each space. A parking space evaluation function is designed to score the available parking spaces. This evaluation function comprehensively considers three indicators: parking path length, number of direction changes, and algorithm time. After normalizing each indicator, a weighted sum is taken to obtain the comprehensive score for each parking space. The parking space with the highest score is selected as the target parking space to improve parking efficiency. The calculation formula for the parking space evaluation function is as follows:

[0010]

[0011] in, For the first The overall score for each parking space. , , These represent the normalized parking path length, number of direction changes, and algorithm time, respectively. , , For the corresponding weight coefficients, satisfying Considering actual driving habits, the number of turns most directly reflects parking difficulty, followed by algorithm time; therefore, the weighting coefficients are chosen to satisfy... .

[0012] Step 3: Construct an improved hybrid A* path planning algorithm

[0013] A smoothness cost and a safety distance cost are introduced into the cost function of the hybrid A* algorithm. The smoothness cost penalizes the change in angle between parent and child nodes to suppress drastic changes in path curvature, making the planned path smoother. The safety distance cost penalizes the path segment based on its distance from the nearest obstacle, applying a larger penalty when the path is close to an obstacle to ensure parking safety. The improved cost function effectively enhances the smoothness and safety of the planned path in narrow environments, solving the problem of poor path quality in narrow environments caused by the traditional hybrid A* algorithm. The improved cost function of the hybrid A* algorithm is as follows:

[0014]

[0015] in, and These are the corners of the parent node and the child node, respectively. and These represent the velocity directions of the parent and child nodes, respectively; a value of 1 indicates forward movement, and a value of -1 indicates backward movement. This refers to the vehicle's wheelbase. For a safe distance.

[0016] Step 4: Two-layer collision detection mechanism

[0017] A two-layer collision detection mechanism ensures path safety. The first layer performs coarse detection based on a double-circle approximation model, covering the front and rear of the vehicle with two circles of equal radius. The distance from the circle's center to the obstacle is calculated to determine if a collision has occurred. If no collision is detected, no further detection is needed to save computational resources. If a collision is detected, the second layer uses a rectangular collision model based on the separating axis theorem for precise collision detection. A bounding box accurately describes the vehicle's outline, and the vehicle and obstacle polygons are projected onto the separating axes. The overlap of these projections determines whether a collision has occurred. This mechanism balances algorithm search speed with avoiding a reduction in the path search space, effectively expanding the search range of feasible paths in narrow parking environments.

[0018] Step 5: Path Post-processing Based on Cubic Spline Interpolation

[0019] Piecewise cubic spline interpolation is applied to the original discrete path nodes planned by the improved hybrid A* algorithm. First, the path nodes are traversed, and the critical points for forward and backward reversal are determined based on the velocity direction of each node, dividing the path into several segments. Cubic spline interpolation is then performed on each segment, satisfying the boundary conditions that the velocity and acceleration at the start, end, and critical points are zero. Through cubic spline interpolation, a globally second-order continuously differentiable smooth path is generated, eliminating steering chatter, meeting the requirements of the vehicle's Ackerman steering model, and significantly improving the trackability of the planned path.

[0020] Step Six: Build the MPC Tracking Controller

[0021] A linear error model is constructed and discretized based on the vehicle's kinematics model, serving as the prediction model for the model predictive controller. Using a smooth path processed by cubic spline interpolation as the reference trajectory, a Taylor series expansion is performed at the reference point, ignoring higher-order terms, to obtain the linear error model. The objective function of the model predictive controller is designed, including the deviation term between the system state variables and the reference trajectory, and a control increment term, considering the front wheel steering angle limit constraints and the front wheel steering angle increment limit constraints. A constrained quadratic programming problem is solved within each control cycle. The first value of the solved control increment sequence is applied to the controlled vehicle, and the process is repeated in the next control cycle to achieve rolling optimization control.

[0022] Step 7: Optimization of Time-Domain Parameters of the Controller Based on Particle Swarm Optimization Algorithm

[0023] A particle swarm optimization (PSO) algorithm is used to optimize the prediction and control time domains of the MPC controller. The fitness function of the PSO algorithm is constructed from three aspects: tracking accuracy, system stability, and ride comfort. It includes six indicators: average position error, maximum position error, root mean square position error, average heading error, average speed error, and control smoothness. These indicators are weighted and summed to obtain a comprehensive fitness value. The PSO algorithm searches for the optimal combination of time-domain parameters within the parameter space, employing a linearly decreasing inertia weight and an early stopping strategy to balance global exploration and local exploitation. The early stopping condition is that the fitness improvement is less than a set threshold for several consecutive generations, and the parameter changes do not exceed a set range. The designed PSO algorithm fitness function is as follows:

[0024]

[0025] in, , , , , , These represent the average position error, maximum position error, root mean square position error, average heading error, average speed error, and control smoothness, respectively.

[0026] Step 8: Path Tracking Control

[0027] The effectiveness of the proposed method was verified in two scenarios: narrow parallel parking and perpendicular parking. The parking path planned by the improved hybrid A* algorithm meets the requirements of smoothness, safety, and feasibility. The MPC tracking controller can track the upper reference trajectory and complete parking tasks in narrow environments.

[0028] Compared with the prior art, the beneficial effects of the present invention are as follows:

[0029] This invention addresses the complex characteristics of dense buildings, limited space, and multiple constraints in narrow parking environments. It introduces smoothness cost and safety distance cost into the cost function of the hybrid A* algorithm, effectively improving the problems of discontinuous curvature and insufficient smoothness in the planning path of the traditional hybrid A* algorithm. At the same time, the safety of the parking path is guaranteed by the safety distance penalty, thus solving the problem of poor path quality in narrow environments.

[0030] This invention employs a two-layer collision detection mechanism. The first layer performs fast coarse detection based on a double-circle approximation model, while the second layer performs precise detection based on a rectangular collision model using the separating axis theorem. This balances the algorithm's search speed while avoiding the reduction of the path search space, enabling the algorithm to effectively search for feasible paths even in narrow parking environments.

[0031] This invention uses piecewise cubic spline interpolation to post-process the planned path, generating a smooth path that is globally second-order continuously differentiable. This eliminates steering shudder, meets the requirements of the vehicle's Ackerman steering model, and significantly improves the trackability of the planned path.

[0032] This invention designs a composite fitness function and uses a particle swarm optimization algorithm to adaptively optimize the prediction and control time domains of the model predictive controller. This overcomes the limitations of traditional methods that rely on manual experience to tune the time domain parameters, and reduces the computational workload while ensuring tracking accuracy.

[0033] Compared to the traditional geometric curve method, this invention does not rely on prior rules and can adapt to different narrow parking scenarios; compared to pure tracking algorithms and linear quadratic regulators, this invention has higher tracking accuracy and constraint handling capabilities; compared to model predictive control without time-domain optimization, this invention achieves a better balance between computational efficiency and tracking accuracy. Attached Figure Description

[0034] Figure 1 This is a schematic diagram of a narrow parallel parking scenario according to the present invention;

[0035] Figure 2 This is a schematic diagram of a narrow vertical parking scenario according to the present invention;

[0036] Figure 3 This is a comparison chart of the path planning results in a narrow parallel parking scenario according to the present invention;

[0037] Figure 4 This is a comparison chart of the path planning results in a narrow vertical parking scenario according to the present invention;

[0038] Figure 5 This is a diagram showing the path tracking results in a narrow parallel parking scenario according to the present invention;

[0039] Figure 6 This is a diagram showing the path tracking results in a narrow vertical parking scenario according to the present invention; Detailed Implementation

[0040] To make the objectives, technical solutions, and advantages of this invention clearer, the invention will be further described in detail below with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are merely illustrative and not intended to limit the invention.

[0041] The specific implementation of the present invention will be described in detail below with reference to specific embodiments. In an embodiment of the present invention, an automatic parking path planning and tracking control method for confined environments includes the following steps:

[0042] Step 1: Environmental Modeling and Parking Space Detection

[0043] like Figure 1 and Figure 2 As shown, a narrow parking scenario generally refers to a parking space whose length or width is slightly larger than the minimum size required for the vehicle to park. The remaining gap between the vehicle and surrounding obstacles when parking and exiting is extremely small, and the passage space is limited and scratches are likely to occur. It is also a typical scenario for measuring the performance of a parking system.

[0044] This invention transforms the automatic parking path planning problem in narrow environments into a multi-constraint optimization problem. The parking path consists of a parking starting point, a target parking space, and several path nodes to be optimized. The feasible solution space is constrained by the parking area boundaries and obstacles. Onboard sensors (such as lidar and ultrasonic radar) are used to perceive the environment, search for available parking spaces, and determine the parking mode based on the length-to-width ratio of the parking space. When the ratio of the length to the width of the parking space is greater than 1, it is determined to be a parallel parking mode; when the ratio is less than 1, it is determined to be a perpendicular parking mode.

[0045] Step 2: Select the best available parking space

[0046] When multiple vacant parking spaces are detected, a hybrid A* algorithm is used to perform a coarse path search for each space. A parking space evaluation function is designed to score the available parking spaces. The parking space evaluation function comprehensively considers three indicators: parking path length, number of direction changes, and algorithm time. The calculation formula is as follows:

[0047]

[0048] in, , , These represent the normalized parking path length, number of direction changes, and algorithm time, respectively. The normalization calculation formulas are as follows:

[0049]

[0050]

[0051]

[0052] in, , , The first The length of the parking path for each parking space, the number of direction changes, and the algorithm time. , , , , , These are the maximum and minimum values ​​of the corresponding indicators.

[0053] Weighting coefficient , , satisfy Choose the parking space with the highest rating as the target parking space.

[0054] Step 3: Construct an improved hybrid A* path planning algorithm

[0055] The basic idea of ​​the hybrid A* algorithm is similar to that of the A* algorithm: starting from the starting point, it searches for child nodes in all directions, calculating the cost of each child node, and selecting the node with the minimum cost as the next parent node, until the destination is reached. Unlike the A* algorithm, the hybrid A* algorithm modifies the node expansion method. Within the allowable range of the vehicle's front wheel steering angle, it divides the node into equal parts, and then, based on a given speed and a vehicle kinematic model, it simulates forward to obtain a path starting from the current node. The endpoint of this path is the child node. Furthermore, the hybrid A* algorithm introduces the vehicle's heading angle as a third dimension, allowing the node expansion direction to rotate according to the heading angle.

[0056] The cost function of the hybrid A* algorithm consists of two parts: ,in This represents the cost from the starting point to the current node. This is a heuristic term, representing the heuristic value from the current node to the endpoint.

[0057] This invention introduces a smoothness cost and a safety distance cost into the cost function of the hybrid A* algorithm. The smoothness cost penalizes the change in angle between parent and child nodes to suppress drastic changes in path curvature. The safety distance cost penalizes the path segment based on its distance from the nearest obstacle, applying a larger penalty when the path is close to an obstacle. The improved cost function expression is:

[0058]

[0059] in, and These are the corners of the parent node and the child node, respectively. and These represent the velocity directions of the parent and child nodes, respectively; forward is represented by 1, and backward by -1. This refers to the vehicle's wheelbase. The formula for calculating the safe distance is as follows:

[0060]

[0061] in, For safety weights, The minimum distance to the nearest obstacle. This is the safe distance threshold.

[0062] Step 4: Two-layer collision detection mechanism

[0063] Because this invention considers a narrow parking environment with obstructed vehicles around the vacant parking spaces, the planned path must ensure that the vehicle avoids collisions with any obstacles in the environment. This invention uses a two-layer collision detection mechanism in the hybrid A* algorithm.

[0064] The first layer performs coarse detection based on a double-circle approximation model, covering the front and rear parts of the vehicle with two circles of the same radius. If the distance from the center of the grid occupied by the obstacle to the center of either circle is greater than the radius, it indicates that a collision will not occur between the vehicle and the obstacle. If no collision is detected, no further detection is needed to save computational resources.

[0065] If the first layer detects a collision, the second layer uses a rectangular collision model based on the separating axis theorem for accurate collision detection, precisely describing the vehicle outline using OBB bounding boxes. The core idea of ​​the separating axis theorem is: if two polygons do not intersect on any projection axis, they do not collide; if they intersect on all projection axes, a collision occurs. The specific detection process is as follows: first, calculate the Euclidean distance between the center points of the two convex polygons; then, select a separating axis and project all vertices of the two convex polygons onto the currently considered separating axis. For each polygon, find the minimum and maximum values ​​of the projection on the separating axis and define the projection radius of the polygon on that axis; finally, compare the projections. If the projections of the two polygons do not overlap on any separating axis, no collision has occurred; otherwise, a collision has occurred.

[0066] Step 5: Path Post-processing Based on Cubic Spline Interpolation

[0067] The original path planned by the hybrid A* algorithm consists of discrete grid nodes. Although it can guarantee obstacle avoidance safety and topological connectivity, it suffers from drawbacks such as curvature discontinuity and control instability. Therefore, this invention uses cubic spline interpolation to post-process the parking path planned by the hybrid A* algorithm.

[0068] First, we give the definition of a cubic spline function: if the function... Satisfy each small interval It is a cubic polynomial, where Given a node, then it is called For nodes A cubic spline function. If at node... Given function value And it satisfies the interpolation conditions. Then it is called It is a cubic spline interpolation function.

[0069] Since the parking path planned by the hybrid A* algorithm may be a combination of forward and backward paths, it is necessary to find the critical points between forward and backward directions in the planned path and perform cubic spline interpolation on the path segments. Assume the path planned by the hybrid A* algorithm is... It consists of nodes, and the critical point is determined as follows: If This indicates that the first Each node is a critical point for reversal, where... For the first The velocity direction of each node is set to 1 for forward movement and -1 for backward movement. By traversing and judging the path nodes, the segmented path can be obtained from the planned path.

[0070] For each path segment, its starting and ending points should satisfy zero velocity and zero acceleration. These boundary conditions are then incorporated into the cubic spline interpolation, specifically the first derivatives at the starting, ending, and critical points. And the second derivative The path smoothness is significantly improved after cubic spline interpolation, generating a smooth path that is globally second-order continuously differentiable, eliminating steering chatter and meeting the requirements of the vehicle's Ackerman steering model.

[0071] Step Six: Build the MPC Tracking Controller

[0072] This invention constructs and discretizes a linear error model based on a vehicle kinematics model, which serves as the prediction model for the model predictive controller. The vehicle kinematics model is established based on the Ackermann steering principle, centered on the rear axle of the vehicle, and its expression is:

[0073]

[0074] in, The coordinates of the rear axle center of the vehicle. For the vehicle's heading angle, For vehicle speed, This refers to the front wheel steering angle. Let's say it's the vehicle's wheelbase. The vehicle's kinematic model can be simplified as... ,in, For the state variables of the system, This refers to the control variables of the system.

[0075] The smoothed path after cubic spline interpolation is used as the reference trajectory, denoted as... , . The formula Expanding the model using Taylor series at the reference point and ignoring higher-order terms, we obtain a linear error model:

[0076]

[0077] in, , Let be the Jacobian matrix. Discretizing the linear error model, we obtain:

[0078]

[0079] In order to control the increment of the control variable, a new state variable is constructed. This yields a new state-space expression:

[0080]

[0081] Based on the above prediction model, the prediction time domain can be derived. The system output within the system. The objective function of the model predictive controller is designed as follows:

[0082]

[0083] in, To predict the time domain, To control the time domain, These are the weighting coefficients. It is a relaxation factor.

[0084] Solve the constrained quadratic programming problem in each control cycle, and obtain the first value of the resulting control increment sequence. Add the control amount from the previous moment Acting on the system, that is Once the next control cycle begins, the above process is repeated to track the parking path.

[0085] Step 7: Optimization of Time-Domain Parameters of the Controller Based on Particle Swarm Optimization Algorithm

[0086] Prediction Time Domain The time step defined by the MPC for predicting future system states directly determines the controller's look-ahead range and the richness of predictive information. (Control Time Domain) The dimensions of the optimization variables are determined, which affects computational complexity. Appropriate selection of time-domain parameters is crucial for ensuring tracking accuracy and real-time performance.

[0087] To achieve better control performance, this invention employs a particle swarm optimization algorithm to optimize the time-domain parameters of the MPC. and Optimization is then performed. The particle swarm optimization algorithm can efficiently handle complex combinatorial optimization problems and quickly obtain near-optimal solutions.

[0088] This invention constructs a fitness function based on three aspects: tracking accuracy, system stability, and ride comfort. First, the first... Tracking error of step:

[0089]

[0090]

[0091]

[0092] in, , , The first The position error, heading error, and velocity error of each step are considered. Six error indicators are then calculated:

[0093]

[0094]

[0095]

[0096]

[0097]

[0098]

[0099] in, , , , , , These represent the average position error, maximum position error, root mean square position error, average heading error, average speed error, and control smoothness, respectively. For longitudinal acceleration, This refers to the steering angle of the front wheels.

[0100] Based on these metrics, the composite fitness function is designed as follows:

[0101]

[0102] The design principle prioritizes ensuring the positional accuracy of path tracking while also considering the smoothness of control and the stability of vehicle attitude. Furthermore, the particle swarm optimization algorithm employs linearly decreasing inertial weights and an early stopping strategy to balance global exploration with local exploitation. The early stopping condition is that the fitness improvement over several consecutive generations is less than a set threshold and the parameter changes do not exceed a set range.

[0103] Step 8: Path Tracking Control

[0104] This invention validates the effectiveness of the algorithm in a narrow parking environment. Two simulation scenarios were designed: narrow parallel parking and narrow perpendicular parking. In all scenarios, obstacle vehicles are parked on both sides of the parking space, forming a narrow parking lane.

[0105] The simulation was validated using a co-simulation platform combining Carsim and Matlab / Simulink. A narrow parking scenario was built in Carsim, with a B-Class Hatchback vehicle selected as the controlled vehicle. An MPC tracking control model was built in Simulink. The parking path, processed by cubic spline interpolation, was selected as the reference path for tracking control.

[0106] like Figures 3-6 As shown, experimental results demonstrate that the parking path planned by the improved hybrid A* algorithm of this invention has excellent smoothness, safety, and feasibility. The model predictive control tracking controller can effectively track the planned path with small errors, and can complete parking tasks in narrow environments.

Claims

1. An automatic parking path planning and tracking control method for narrow environments, characterized in that, Includes the following steps: Step 1: Environmental Modeling and Parking Space Detection The problem of automatic parking path planning in narrow environments is transformed into a multi-constraint optimization problem. The parking path consists of a parking start point, a target parking space, and several path nodes to be optimized. The feasible solution space is constrained by the parking area boundary and obstacles. Onboard sensors are used to perceive the environment, search for available parking spaces, and determine the parking mode based on the length-to-width ratio of the parking space. When the ratio of the length to the width of the parking space is greater than 1, it is judged as a parallel parking mode; when the ratio of the length to the width of the parking space is less than 1, it is judged as a perpendicular parking mode. Step 2: Select the best available parking space When multiple vacant parking spaces are detected, a hybrid A* algorithm is used to perform a coarse path search for each parking space. A parking space evaluation function is designed to score the available parking spaces. The parking space evaluation function comprehensively considers three indicators: parking path length, number of reversals, and algorithm time. The parking space with the highest score is selected as the target parking space. Step 3: Construct an improved hybrid A* path planning algorithm The cost function of the hybrid A* algorithm introduces a smoothness cost and a safety distance cost. The smoothness cost penalizes the change in the angle between the parent node and the child node to suppress drastic changes in the path curvature. The safety distance cost penalizes the path segment based on the distance to the nearest obstacle to ensure parking safety. The improved cost function can effectively improve the smoothness and safety of the planned path in narrow environments. Step 4: Two-layer collision detection mechanism A dual-layer collision detection mechanism is adopted to ensure path safety. The first layer performs coarse detection based on a double-circle approximation model, using two circles with the same radius to cover the front and rear parts of the vehicle. If no collision is detected, no further detection is required to save computational resources. If a collision is detected, the second layer uses a rectangular collision model based on the separating axis theorem for accurate collision detection. The directional bounding box accurately describes the vehicle outline, expanding the path search space in narrow parking environments while ensuring obstacle avoidance accuracy. Step 5: Path Post-processing Based on Cubic Spline Interpolation The original discrete path nodes planned by the improved hybrid A* algorithm are processed by piecewise cubic spline interpolation. First, the path nodes are traversed to determine the critical points for forward and backward reversal. The path is divided into several segments, and cubic spline interpolation is performed on each segment. The boundary conditions of zero velocity and zero acceleration at the start point, end point and critical point are met to generate a smooth path that is second-order continuous and differentiable in the whole domain, eliminating steering chatter and meeting the requirements of the vehicle Ackerman steering model. Step Six: Build the MPC Tracking Controller A linear error model is constructed and discretized based on the vehicle kinematics model. The smooth path after cubic spline interpolation is used as the reference trajectory. The objective function of the MPC controller is designed. The objective function includes the deviation term between the system state variables and the reference trajectory and the control increment term. A constrained quadratic programming problem is solved in each control cycle. The first value of the solved control increment sequence is applied to the controlled vehicle to achieve rolling optimization control. Step 7: Optimization of Time-Domain Parameters of the Controller Based on Particle Swarm Optimization Algorithm The particle swarm optimization algorithm is used to optimize the prediction and control time domains of the MPC controller. A composite fitness function is designed, which includes six indices: mean position error, maximum position error, root mean square position error, mean heading error, mean velocity error, and control smoothness. The particle swarm optimization algorithm is used to search for the optimal combination of time domain parameters in the parameter space. Step 8: Path Tracking Control The improved hybrid A* algorithm was validated in two scenarios: parallel parking and perpendicular parking. The parking path planned by the improved algorithm meets the requirements of smoothness, safety and feasibility. The position error, lateral error and heading error of the MPC tracking controller are small, and it can complete the parking task in narrow environment.

2. The automatic parking path planning and tracking control method for narrow environments according to claim 1, characterized in that, In step two, the parking space evaluation function processes three indicators—parking path length, number of direction changes, and algorithm time—using a normalization method. The weighting coefficients prioritize minimizing the number of direction changes, followed by minimizing algorithm time. The parking space evaluation function is as follows: in, For the first The overall score for each parking space indicates that the higher the score, the easier it is to park in that space. , and For the weighting coefficients, satisfying . , , These are the normalized calculation formulas for parking path length, number of reversals, and algorithm time, respectively.

3. The automatic parking path planning and tracking control method for narrow environments according to claim 1, characterized in that, In step three, the smoothness cost is achieved by penalizing the change in the front wheel steering angle between the parent and child nodes. The safety distance cost is penalized using an exponential function based on the distance between the path segment and the nearest obstacle; the closer the distance, the greater the penalty. The cost function of the improved hybrid A* algorithm is shown below: in, and These are the corners of the parent node and the child node, respectively. and These represent the velocity directions of the parent and child nodes, respectively; a value of 1 indicates forward movement, and a value of -1 indicates backward movement. This refers to the vehicle's wheelbase. The formula for calculating the safe distance is as follows: in, For safety weights, The minimum distance to the nearest obstacle. This is the safe distance threshold.

4. The automatic parking path planning and tracking control method for narrow environments according to claim 1, characterized in that, In step four, the radius of the double-circle approximation model is determined by the vehicle's length and width, and the positions of the two center points are calculated based on the vehicle's rear axle center coordinates and heading angle. The specific steps for constructing the dual-layer collision detection mechanism are as follows: First, a coarse detection is performed based on the double-circle approximation model. If no collision is detected, no further collision detection is needed. Conversely, if a collision is detected, a rectangular collision model is used for precise collision detection.

5. The automatic parking path planning and tracking control method for narrow environments according to claim 1, characterized in that, In step five, cubic spline interpolation applies boundary conditions where the first derivative and the second derivative are both zero to the starting point, ending point, and reversal critical point of each path segment.

6. The automatic parking path planning and tracking control method for narrow environments according to claim 1, characterized in that, The constraints of the model predictive controller in step six include the front wheel steering angle limit constraint and the front wheel steering angle increment limit constraint. The objective function of the MPC tracking controller is specifically expressed as follows: in, To predict the time domain, To control the time domain, These are the weighting coefficients. Using the relaxation factor, within each control cycle, solving the optimization problem shown in the above equation yields a series of control increments. The first control increment is added to the control quantity from the previous moment and applied to the system. This process is repeated in the next control cycle to achieve parking path tracking.

7. The automatic parking path planning and tracking control method for narrow environments according to claim 1, characterized in that, In step seven, the fitness function of the particle swarm optimization algorithm includes six metrics and employs a linearly decreasing inertia weight and an early stopping strategy. The early stopping condition is that the fitness improvement is less than a set threshold for several consecutive generations and the parameter changes do not exceed a set range. The designed particle swarm optimization algorithm fitness function is as follows: in, , , , , , These represent the average position error, maximum position error, root mean square position error, average heading error, average speed error, and control smoothness, respectively.

Images