Safety Distance Model Research Articles

Research on recognition of slippery road surface and collision warning system based on deep learning

Aiming at the problems of slow detection speed, large prediction error and weak environmental adaptability of current vehicle collision warning system, this paper proposes a recognition method of slippery road surface and collision warning system based on deep learning. Firstly, this paper uses the on-board camera to monitor the environment and road conditions in front of the vehicle in real time, and a residual network model FS-ResNet50 is proposed, which integrated SE attention mechanism and multi-level feature information based on the traditional ResNet50 model. The FS-ResNet50 model is used to identify the slippery states of the current road, such as wet and snowy. Secondly, the yolov5 algorithm is used to detect the position of the vehicle in front, and a driving safety distance model with adaptive traffic environment characteristics is established based on different road conditions and driving conditions, and an early warning area that dynamically changed with the speed and the road slippery states is generated. Finally, according to the relationship between the warning area and the position of the vehicle, the possible collision is predicted and timely warned. Experimental results show that the method proposed in this paper improves the overall warning accuracy by 6.72% and reduces the warning false alarm rate for oncoming traffic on both sides by 16.67% compared with the traditional collision warning system. It can ensure safe driving, especially in bad weather conditions and has a high application value.

Risk-Aware Lane Change and Trajectory Planning for Connected Autonomous Vehicles Based on a Potential Field Model

To enhance the safety of lane changes for connected autonomous vehicles in an intelligent transportation environment, this study draws from potential field theory to analyze variations in the risks that vehicles face under different traffic conditions. The safe minimum vehicle distance is dynamically adjusted, and a comprehensive vehicle risk potential field model is developed. This model systematically quantifies the risks encountered by connected autonomous vehicles during the driving process, providing a more accurate assessment of safety conditions. Subsequently, vehicle motion is decoupled into lateral and longitudinal components within the Frenet coordinate system, with quintic polynomials employed to generate clusters of potential trajectories. To improve computational efficiency, trajectory evaluation metrics are developed based on vehicle dynamics, incorporating factors such as acceleration, jerk, and curvature. An initial filtering process is applied to these trajectories, yielding a refined set of candidates. These candidate trajectories are further assessed using a minimum safety distance model derived from potential field theory, with optimization focusing on safety, comfort, and efficiency. The algorithm is tested in a three-lane curved simulation environment that includes both constant-speed and variable-speed lane change scenarios. Results show that the collision risk between the target vehicle and surrounding vehicles remains below the minimum safety distance threshold throughout the lane change process, ensuring a high level of safety. Furthermore, across various driving conditions, the target vehicle’s acceleration, jerk, and trajectory curvature remained well within acceptable limits, demonstrating that the proposed lane change trajectory planning algorithm successfully balances safety, comfort, and smoothness, even in complex traffic environments.

Analysis of mixed traffic flow characteristics based on cellular automata model under lane management measures

In the future, mixed traffic flow comprising Human-Driven Vehicles (HDVs), Connected and Autonomous Vehicles (CAVs), and CAV platoons will coexist for an extended period. Exploring the operational characteristics of mixed traffic flow under different lane management measures is essential for effective management and control. Initially, we analyze the motion characteristics of HDVs, CAVs, and CAV platoons and identify the car-following types within mixed traffic. Based on vehicle motion characteristics and the variance in maximum desired speeds among HDV drivers, we establish longitudinal motion rules for HDVs using a safety distance model. For CAVs and their platoons, we develop longitudinal motion rules by considering platoon merging and splitting behaviors, as well as speed and spacing error requirements for platoon driving. Subsequently, we formulate lateral lane-change rules based on the Symmetric Two-lane Cellular Automata (STCA) model, considering differences in reaction times and following distances between HDVs and CAVs. Finally, we conducted simulation experiments on a unidirectional three-lane highway using the multi-lane mixed traffic flow cellular automata model, analyzing the characteristics of mixed traffic under various lane management measures, such as mixed lane, CAV-dedicated lane, and CAV-priority lane. The results show that as the proportion of CAV (denoted as p) increases, the traffic flow capacity, optimal density, and jam density also increase. When p≤0.6, the k−q diagram exhibits a triangular shape, whereas for p>0.6, it assumes a trapezoidal shape. Implementing CAV-dedicated lane can reduce Cooperative Adaptive Cruise Control (CACC) degradation rates but only enhances traffic flow capacity when the CAV proportion reaches a certain threshold. Compared to the mixed lane scheme, when p≤0.3 and k≤0.3, the CAV-priority lane schemes not only meet traffic demands but also reduce CACC degradation rates. The vehicle speed in CAV-priority lane surpasses that in HDV lanes, facilitating improved traffic efficiency for CAVs. The distribution of maximum speeds among HDV drivers affects the fundamental diagram of mixed traffic flow and the performance of CAV-priority lane, with greater impacts observed as the standard deviation of the HDVs' maximum speed increases.

Research on the strategy of cruise control system for urban traffic jam assistant

This paper proposes a hierarchical cruise control strategy that can adapt to urban traffic scenarios. Firstly, the perception layer defines a target selection strategy by using a novel trapezoidal dangerous area. Then, the upper controller contains speed follow mode and distance follow mode. An improved PI mode with integral separation and saturation terms is developed to achieve the desired speed following control, a safe distance mode via variable time headway is developed to facilitate the safe and smooth distance following control. The lower controller is designed based on feedforward and PI feedback. Finally, the experimental results demonstrate that the strategy can effectively select suitable target vehicles in complex urban scenarios. With the improved safe distance model and the third-order Bessel actuator response delay model, the strategy exhibits adaptability and stability. This study offers valuable suggestions and guidance for designing and applying the cruise control strategy for urban traffic scenarios.

Collision Avoidance Path Planning and Tracking Control for Autonomous Vehicles Based on Model Predictive Control.

In response to the fact that autonomous vehicles cannot avoid obstacles by emergency braking alone, this paper proposes an active collision avoidance method for autonomous vehicles based on model predictive control (MPC). The method includes trajectory tracking, adaptive cruise control (ACC), and active obstacle avoidance under high vehicle speed. Firstly, an MPC-based trajectory tracking controller is designed based on the vehicle dynamics model. Then, the MPC was combined with ACC to design the control strategies for vehicle braking to avoid collisions. Additionally, active steering for collision avoidance was developed based on the safety distance model. Finally, considering the distance between the vehicle and the obstacle and the relative speed, an obstacle avoidance function is constructed. A path planning controller based on nonlinear model predictive control (NMPC) is designed. In addition, the alternating direction multiplier method (ADMM) is used to accelerate the solution process and further ensure the safety of the obstacle avoidance process. The proposed algorithm is tested on the Simulink and CarSim co-simulation platform in both static and dynamic obstacle scenarios. Results show that the method effectively achieves collision avoidance through braking. It also demonstrates good stability and robustness in steering to avoid collisions at high speeds. The experiments confirm that the vehicle can return to the desired path after avoiding obstacles, verifying the effectiveness of the algorithm.

Integrating situation-aware knowledge maps and dynamic window approach for safe path planning by maritime autonomous surface ships

This study investigates the enhancement of Maritime Autonomous Surface Ships (MASS) navigation and path-planning through the integration of ontology-based knowledge maps (KM) with the Dynamic Window Approach (DWA), a fusion termed KM-DWA. The ontology-based KM model is important for MASS navigation, offering a framework for situational awareness, including contextual information fusion and decision-making evidence. This research enriches the KM model with collision avoidance rules from the International Regulations for Preventing Collisions at Sea (COLREGs), building upon our previous work on MASS's efficient and COLREGs-compliant navigation in encounter scenarios. The model provides navigational context, covers COLREGs rules and environmental factors, and recommends MASS actions for various scenarios as suggested by COLREGs. Moreover, an adapted DWA, tailored to maritime navigation, accounts for specific constraints and safety measures for MASS, utilising KM-derived situational awareness as constraints in its cost function for path planning. A significant innovation introduced here is a tiered safety distance model featuring proactive, defensive, and collision buffers to ensure rule-compliant and effective collision avoidance. This scheme enables MASS to take timely collision avoidance actions at both proactive and defensive distances, in line with COLREGs recommendations. The effectiveness of the KM-DWA algorithm is validated by comparing it with the basic DWA algorithm in single- and multi-vessel encounter scenarios. The experiment outcomes illustrate the integrated approach's superiority in terms of COLREGs compliance and collision avoidance rate, emphasising its ability to support COLREGs-compliant decision-making and enhance situational awareness in autonomous maritime operations.

Vehicle adaptive cruise control strategy based on improved safe distance model for off-roads with changeable pavement and slope

Compared with the structured environment, off-road environment has complex and ever-changing road conditions. This paper is focused on the vehicle adaptive cruise task for vehicles driving on complex off-road terrain. Traditional ACC strategies do not take the complex road conditions into consideration, thus easily make the relative distance between vehicles unreasonable. To solve this problem, an off-road adaptive cruise control (OACC) strategy is proposed for off-roads with changeable pavement and slope. Firstly, the influence of road conditions and vehicle relative motion states on safe distance between vehicles have analyzed with the aim of developing a more reasonable space strategy for off-road conditions. Then, based on the analysis results, an improved safe distance model (ISDM) is proposed which take the influence factors into account comprehensively. The concept of road impact factor is proposed to prevent model degradation and effectively balance the influence of single road conditions and comprehensive factors on safe distance. Besides, target to improve the adaptability of ACC, a novel state space model has developed which can handle not only the change of vehicle motion states but also road conditions. In addition, the MPC-based OACC for off-road environment is proposed, which can enable the vehicle to better adapt to relative motion states and road conditions based on the novel state space model. Finally, the performance of OACC is verified by co-simulation in MATLAB/Simulink and Carsim, and a hardware-in-the-loop simulation system. Furthermore, the analysis of ISDM has conducted to illustrate the differences and similarities compared with the traditional safe distance models and to verify the effectiveness of ISDM. Simulation results show that the OACC and ISDM proposed in this paper have great performance in different off-road working conditions.

Design of Automotive Collision Avoidance Lower Position Controller Based on GA Optimization

Abstract To improve the control stability of the lower controller in the vehicle collision avoidance system, and solve the problem of traditional fuzzy PID not providing good dynamic performance under large parameter changes, a lower controller for vehicle collision avoidance based on genetic algorithm optimized fuzzy proportional-integral-derivative (PID) control was studied. The vehicle collision avoidance system collects real-time motion information of the preceding vehicle through millimeter-wave radar and calculates the safety distance. The safety distance model and upper controller of the system calculate the required acceleration of the vehicle. Afterwards, the expected acceleration of the vehicle is controlled by the lower controller of fuzzy PID. At the same time, the improved GA is used to adjust the PID parameters, solving the problem of poor robustness of the lower controller. A simulation platform was built to simulate the longitudinal vehicle collision avoidance control system jointly. The simulation results show that the lower controller designed based on this control algorithm has better stability and meets the collision avoidance requirements, providing an important basis for the development of vehicle control systems.

Quantifying risks of lane-changing behavior in highways with vehicle trajectory data under different driving environments

Improper lane-changing behavior may have a great impact on the safety of the surrounding vehicles, and accurate risk assessment of lane-changing behavior can detect improper lane-changing behavior. Many existing risk assessment methods are based on the traditional minimum safe distance model as the theoretical foundation, not taking into account the effects of different weather and road conditions. Moreover, the initially calculated safe lane-changing distance through the model is generally the limit safety distance, which deviates to some extent from the initial distance maintained when the vehicle begins to change lanes during the actual driving process. The ratio of these two distances can reflect the degree of risk between the changing vehicle and its neighboring vehicles during lane-changing. For this reason, a lane-changing safety coefficient is defined and lane-changing risk assessment model based on the hyperbolic tangent function is constructed by combining the change characteristics of this coefficient. An environmental adjustment factor was introduced into the model to consider the influence of different driving weather and road conditions on the lane-changing risk assessment. Then, based on survey data from the survey questionnaire, the weights of the influencing factors were determined by using the hierarchical analysis method, and the environmental adjustment factor in the model was calibrated by using the fuzzy comprehensive assessment method. Finally, to verify the effectiveness of the model, the lane-changing trajectories of 373 vehicles were extracted from the HighD dataset. The risk assessment value of lane-changing was calculated by using the constructed model and was classified into high-risk, medium-risk and low-risk by using the K-means++ clustering algorithm. Various evaluation results were compared with the evaluation results from the expert evaluation method and the classical risk evaluation indexes, and the evaluation results are basically the same, which verifies the effectiveness of the lane-changing risk model established.

A cellular automaton model for mixed traffic flow considering the size of CAV platoon

In the environment of intelligent connected vehicles, the mixed traffic flow consisting of human-driven vehicles (HDVs), connected and automated vehicles (CAVs), and their platoons will coexist for a long time. Exploring and discovering the operational rules of mixed traffic flow is the foundation for its management and control. This study delves into the distinct characteristics of various vehicles within a mixed traffic environment, particularly focusing on HDVs, CAVs, and CAV platoons. Drawing on the psychological predisposition of HDVs to maintain a prudent following distance, we introduce a discrete motion safety distance model to establish the longitudinal movement rule for HDVs. Furthermore, a longitudinal movement rule for CAVs is established based on their ability to keep a constant headway using Adaptive Cruise Control (ACC) and Cooperative Adaptive Cruise Control (CACC), alongside meeting specific speed and spacing criteria for platoon movement. On this basis, the paper proposes a cellular automaton model for single-lane mixed traffic flow that accounts for CAV platoon length and the decision-time disparity between HDVs and CAVs. The simulation results show that an increased proportion of CAVs can enhance the traffic capacity, elevate average speed, extend average platoon length, and reduce the congestion ratio and CACC degradation rate. Notably, in scenarios where the CAV proportion p > 0.7, the flow-density graph shows a trapezoidal shape, with the majority of CAVs in the traffic flow synchronizing their movements in CACC mode. Even when vehicular density surpasses the optimum threshold, the traffic flow maintains high-speed operation until reaching a critical congestion point, whereupon velocities precipitously decline. Through sensitivity analysis aimed at understanding the impact of maximum platoon length, it has been observed that, for CAV proportions below 0.7, variations in maximum platoon length negligibly impact traffic capacity, velocity, CACC degradation, and congestion levels. Only in scenarios dominated by CAVs does extending platoon length restrictions significantly enhance performance. However, as the maximum platoon length increases, the gains in traffic capacity, speed, CACC degradation rate, and congestion ratio diminish. When the platoon length of CACC exceeds a certain threshold, there will not be a significant improvement in traffic capacity or speed.

Research on AEB-P Control Strategy Considering Braking Process Comfort

A control approach for the pedestrian-oriented automobile automatic emergency braking system (AEB-P) that takes driving comfort into account is suggested in an effort to further optimize the AEB-P system's control algorithm. Firstly, for the AEB-P assessment standard of C-NCAP, a braking safety distance model that takes the driver's comfort into account during braking is created. The next step was to construct a hierarchical controller using PID and MPC principles. Lastly, CarSim and Matlab/Simulink are used to build and validate the test scenarios and control methods. The simulation results demonstrate that both ride comfort and driving safety can be guaranteed by the suggested control technique.

An intelligent human-machine interaction-based longitudinal control strategy for autonomous vehicles

In the foreseeable future, the anticipation is that intelligent vehicles will transition to a mode where the intelligent driving system collaborates seamlessly with the human driver. This harmonious integration between the driver and the intelligent control system holds paramount significance for the successful execution of driving tasks, ultimately contributing to the development of more advanced and user-friendly automobiles. A pivotal element in advancing from assisted to autonomous driving lies in the establishment of a human-machine co-driving mode. This research delineates a longitudinal control strategy tailored for intelligent vehicles featuring human-machine interaction. The approach involves the creation of a personalized safe distance model for car-following by collecting driver characteristic parameters. Focused on the car-following methodology, this study formulates the kinematics state space equation, performance index function, and constraint conditions governing car-following dynamics. Subsequently, a car-following control strategy is devised based on model predictive control (MPC), which is addressed through rolling optimization techniques. Building upon this foundation, a human-machine driving control strategy is proposed to dynamically allocate driving authorities in real-time. This strategy takes into account speed and vehicle distance risk as two-dimensional inputs, employing a cooperative driving control strategy within the dual-drive dual-control system. The proposed method was validated in a simulated environment.

A New Safety Distance Model Based on Braking Process Considering Adhesion Coefficient

The traditional safety distance model based on the braking process is inadequate for scenarios where the adhesion coefficient varies. This research presents a safety distance model that takes into account the real-time fluctuations in the adhesion coefficient during the braking process. The proposed safety distance model is the basis for a three-level control strategy aimed at ensuring the safety and stability of the car-following process. A simulation is conducted to model the variation of the road’s adhesion coefficient during car braking. The experimental results, obtained for various initial adhesion coefficients and speeds, demonstrate that the precision of the safety distance calculated is improved by a minimum of 14.99% when compared to the traditional safety distance. Furthermore, the simulation results of the car-following process of a motorcade consisting of five cars provide evidence of the safety and stability of the suggested model.

An Efficient Trajectory Planning Approach for Autonomous Ground Vehicles Using Improved Artificial Potential Field

In this paper, the concept of symmetry is utilized in the promising trajectory planning design of autonomous ground vehicles—that is, the construction and the solution of improved artificial potential field-based trajectory planning approach are symmetrical. Despite existing artificial potential fields (APF) achievements on trajectory planning in autonomous ground vehicles (AGV), applying the traditional approach to dynamic traffic scenarios is inappropriate without considering vehicle dynamics environment and road regulations. This paper introduces a highly efficient approach for planning trajectories using improved artificial potential fields (IAPF) to handle dynamic road participants and address the issue of local minima in artificial potential fields. To begin with, potential fields are created with data obtained from other sensors. By incorporating rotational factors, the potential field will spin when the obstacle executes a maneuver; then, a safety distance model is also developed to limit the range of influence in order to minimize the computational burden. Furthermore, during the planning process, virtual forces using the gradient descent method are generated to direct the vehicle’s movement. During each timestep, the vehicle will assess whether it is likely to encounter a local minimum in the future. Once a local minimum is discovered, the method will create multiple temporary objectives to guide the vehicle toward the global minimum. Consequently, a trajectory that is both collision-free and feasible is planned. Traffic scenarios are carried out to validate the effectiveness of the proposed approach. The simulation results demonstrate that the improved artificial potential field approach is capable of generating a secure trajectory with both comfort and stability.

Obstacle avoidance decision and trajectory tracking control of intelligent vehicle considering surrounding vehicles

To enhance the obstacle avoidance performance and trajectory tracking control stability of vehicles in emergency scenarios, and to solve the problem that the existing decision-making methods based on the minimum safe distance model cannot effectively make decisions in some scenarios, a novel lane-changing obstacle avoidance decision-making and control method is proposed. Initially, when emergency braking cannot avoid obstacles safely, the lane-changing trajectory is planned by quintic polynomial, and the minimum distance and collision detection (MDCD) algorithm is then employed to ascertain whether the lane-changing trajectory meets the safety conditions. Subsequently, a Linear Time-Varying Model Predictive Control (LTV MPC) trajectory tracking controller is designed based on the 7-DOF vehicle dynamics model. Finally, the effectiveness of the proposed method is verified through two representative scenarios. The simulation results indicate that the MDCD algorithm can ensure the safety and effectiveness of obstacle avoidance decisions in all emergency obstacle avoidance scenarios, and when tracking the emergency lane-changing trajectory in different typical scenarios, compared with the LTV MPC controller based on the single-track vehicle dynamics model (STVDM), the maximum tracking error can be reduced by 50.7% and 60.1%, respectively, while ensuring near-equivalent real-time performance operation.

Dynamic Illumination Method for Rural Highway Intersections with Traffic Flow Changes

The conflicts between vehicles and vulnerable road users are frequent at rural highway intersections. Improving lighting conditions at intersections can significantly reduce the likelihood of nighttime accidents. This study aims to develop a dynamic illumination method for rural highway intersections with traffic flow changes. First, the safety design principles of a lighting scheme were formulated for minimum illuminance, vehicle speed, and traffic risk at intersections. In addition, a traffic risk quantification model for intersections was developed with illuminance and traffic flow parameters as inputs, incorporating gap acceptance theory and the braking safety distance model. Second, combining safety design principles and a traffic risk quantification model, a calculation method for optimal illuminance at intersections was designed with minimum illuminance as the optimization objective, and considering the risk at intersections not exceeding the safety threshold as the constraint. Finally, a case study was conducted in a simulated environment. Evaluation results demonstrate that limiting the maximum nighttime vehicle speed at intersections to 50 km/h can maximize the energy-saving rate of the lighting system while ensuring traffic safety and transport efficiency. The advantage of the method is that it can calculate the safe lighting scheme for highway intersections without any historic accident data and save lighting energy consumption. It increases the feasibility of promoting lighting facilities at rural highway intersections.

Forward Collision Warning Strategy Based on Millimeter-Wave Radar and Visual Fusion

Forward collision warning (FCW) is a critical technology to improve road safety and reduce traffic accidents. However, the existing multi-sensor fusion methods for FCW suffer from a high false alarm rate and missed alarm rate in complex weather and road environments. For these issues, this paper proposes a decision-level fusion collision warning strategy. The vision algorithm and radar tracking algorithm are improved in order to reduce the false alarm rate and omission rate of forward collision warning. Firstly, this paper proposes an information entropy-based memory index for an adaptive Kalman filter for radar target tracking that can adaptively adjust the noise model in a variety of complex environments. Then, for visual detection, the YOLOv5s model is enhanced in conjunction with the SKBAM (Selective Kernel and Bottleneck Attention Mechanism) designed in this paper to improve the accuracy of vehicle target detection. Finally, a decision-level fusion warning fusion strategy for millimeter-wave radar and vision fusion is proposed. The strategy effectively fuses the detection results of radar and vision and employs a minimum safe distance model to determine the potential danger ahead. Experiments are conducted under various weather and road conditions, and the experimental results show that the proposed algorithm reduces the false alarm rate by 11.619% and the missed alarm rate by 15.672% compared with the traditional algorithm.

Research on Automatic Emergency Braking System Based on Target Recognition and Fusion Control Strategy in Curved Road

To address the issue of incorrect recognition in the automatic emergency braking (AEB) systems on curved roads, a target recognition model is proposed to obtain the road curvature and to calculate the relative lateral distance. Based on the information from the ego vehicle and the preceding vehicles, the accurate selection of the hazardous target is accomplished. After identifying the dangerous target, a control strategy based on the fusion algorithm is proposed, because the safety distance model and the Time-to-Collision (TTC) model both have their limitations and cannot ensure driving safety and comfort simultaneously. The TTC model is optimized according to the actual relative distance between two vehicles on curved roads, the graded warning strategy and braking intervention time are established by the TTC model. And then the graded braking strategy is designed according to the safety distance model. The simulation platform is built based on Carsim and Simulink for verification and analysis. The results demonstrate that the proposed AEB control strategy on curved roads can accurately and efficiently identify the target vehicles on curved roads, avoid false triggering issues, and improve the AEB system’s reliability. And effectively avoid collisions with target vehicles that are in the same lane, improving driving safety and comfort.

Research on Lane-Change Decision and Planning in Multilane Expressway Scenarios for Autonomous Vehicles

Taking into account the issues faced by self-driving vehicles in multilane expressway scenarios, a lane-change decision planning framework that considers two adjacent lanes is proposed. Based on this framework, the lateral stability of an autonomous vehicle under near-limit conditions during lane change is studied by the phase-plane method. Firstly, a state-machine-based driving logic is designed and a decision method is proposed to design the lane-change intention based on the surrounding traffic information and to consider the influence of the motion state of other vehicles in the adjacent lanes on the self-driving vehicle. In order to realize adaptive cruising under the full working conditions of the vehicle, a safety distance model is established for different driving speeds and switching strategies for fixed-speed cruising, following driving, and emergency braking are developed. Secondly, for the trajectory planning problem, a lane-change trajectory based on a quintuple polynomial optimization method is proposed. Then, the vehicle lateral stability boundary is investigated; the stability boundary and rollover boundary are incorporated into the designed path-tracking controller to improve the tracking accuracy while enhancing the rollover prevention capability. Finally, a simulation analysis is carried out through a joint simulation platform; the simulation results show that the proposed method can ensure the driving safety of autonomous vehicles in a multilane scenario.

Hierarchical collision-free trajectory planning for autonomous vehicles based on improved artificial potential field method

To enable autonomous vehicles to generate smooth and collision-free trajectories and improve their driving performance on structured roads, this paper proposes a hierarchical trajectory planning algorithm based on an improved artificial potential field method. To improve the applicability of the algorithm to complex scenarios, the Frenet coordinate system was established to address these limitations. First, the safety distance model is applied to the risk assessment of the improved artificial potential field method. Then, the hierarchical solution is carried out, and the road solvable convex space and the rough path solution are solved by combining the artificial potential field method. On this basis, the potential field term and the smoothing term cost function are established, and the sequential quadratic programming (SQP) algorithm is used to solve the exact path that meets the requirements of safety and smoothness. Hierarchical planning shortens the solution time by quickly determining the bounds of the convex space. Finally, in the speed planning, in order to take into account the comfort and safety of the occupants, the speed curve is solved by considering the dynamic constraints of the vehicle. The obstacle avoidance effects of the algorithm on static and dynamic obstacles are tested in different simulation scenarios. The results of the simulation experiment show that the proposed algorithm can successfully achieve obstacle avoidance on complex structured roads.