A motion planning system and method in uncertain environment based on factor graph

Abstract

The present invention discloses a motion planning system and method in an uncertain environment based on a factor graph, and belongs to the technical field of automatic driving and motion planning. The system includes a perception module, a map and positioning module, a global path planning module, a local path planning module and a control module, wherein the perception module is used to obtain obstacle information, the map and positioning module is used to provide the position information of the vehicle, the global path planning module is used to plan a global path from the position of the vehicle to the target point, the local path planning module is used to use the factor graph method for planning, and output the trajectory of the vehicle in the next few seconds, and the control module is used to calculate the acceleration and the angle of rotation based on the trajectory of the next few seconds given by the local planning module and the position information of the vehicle, and send them to the vehicle chassis or simulator. The present invention can process the uncertainty of perception and prediction in the automatic driving system in real time, thereby generating a safer and more reasonable driving trajectory in a complex traffic environment.

Description

Technical Field

[0001] The present invention belongs to the technical field of autonomous driving and motion planning, and relates to a motion planning system and method in an uncertain environment based on a factor graph. Background Art

[0002] The autonomous driving system is a complex system engineering, which is usually composed of four main modules: perception, prediction, decision-making planning, and control. The perception module obtains environmental information through sensors such as cameras, radars, and lidars, and provides the system with perception data of the surrounding environment; prediction is an estimate of the future state of the environment, including the prediction of the behavior of other vehicles and pedestrians; the decision-making and planning module makes reasonable driving decisions based on perception data, traffic rules, vehicle status, and prediction of other vehicles, and plans a safe, efficient, and comfortable driving path. The control module is responsible for following the planned trajectory and performing operations such as acceleration, deceleration, and steering of the vehicle; in this system, uncertainty exists in each module of the system. First, the environmental perception of the perception module is not always completely accurate, especially in complex traffic environments, where the position of other vehicles and the perception of their own status may be inaccurate. In addition, due to the unpredictability of dynamic traffic scenes, the behavior of other traffic participants is full of uncertainty, which brings additional challenges to decision-making and planning.

[0003] Current motion planning algorithms often simplify the handling of perception and prediction uncertainty. Although this simplification can provide a relatively simple solution in many cases, it may seriously affect the safety and rationality of trajectory planning in practical applications. Especially in complex and dynamic traffic scenarios, the uncertainty caused by factors such as environmental changes, sensor errors, and unpredictable behavior of other traffic participants is often ignored or simplified to a fixed value, which cannot accurately reflect the actual situation. This practice of not fully considering uncertainty will cause the autonomous driving system to be unable to make the best decision when facing complex environments, and may even result in dangerous trajectory planning, increasing the risk of collisions or other accidents between the vehicle and other traffic participants.

[0004] Although some motion planning algorithms attempt to account for uncertainty, most rely on model predictive control (MPC) techniques to deal with this problem. Model predictive control methods are able to account for the effects of uncertainty to a certain extent and optimize the trajectory by continuously adjusting the control input. However, MPC methods usually require repeated numerical optimization solutions to achieve trajectory planning, which leads to high computational complexity and significant consumption of computing resources. With the dynamic changes in the environment and traffic conditions, the MPC method may require a large number of computational iterations, especially in scenarios with high uncertainty. This process will consume more time and computing power, thus affecting real-time performance and system response speed. This high computing demand will limit its application in real-time autonomous driving systems, especially in complex scenarios that require fast response and real-time decision-making, and may not meet the computing requirements of autonomous driving systems.

[0005] Therefore, existing motion planning algorithms have significant limitations when dealing with perception and prediction uncertainties. They fail to fully consider various uncertain factors in the environment and fail to achieve high efficiency in computing resources, which in turn affects the application of autonomous driving systems in complex and dynamic environments. Summary of the invention

[0006] The purpose of the present invention is to provide a motion planning system and method in an uncertain environment based on a factor graph, which aims to effectively handle the perception and prediction uncertainties in the autonomous driving system, and generate the optimal trajectory at a lower computational cost, so as to ensure that the autonomous driving system can make more reasonable and safe decisions in various complex traffic scenarios, thereby improving the safety and reliability of the autonomous driving system in complex traffic scenarios. By designing a motion planning method that can dynamically consider the uncertainty of traffic participants in the environment, the present invention can generate reasonable trajectories in real time when faced with dynamic changes and uncertain information, and avoid potential risks caused by perception errors or inaccurate predictions. In addition, the present invention also optimizes computing efficiency, overcomes the problems of high computing power and high latency in traditional methods, and ensures that autonomous driving vehicles can make accurate motion decisions in environments with high uncertainty.

[0007] The technical solution of the present invention is:

[0008] A motion planning system in an uncertain environment based on a factor graph includes a perception module, a map and positioning module, a global path planning module, a local path planning module and a control module, wherein the perception module is used to obtain obstacle information, the map and positioning module is used to provide vehicle position information, the global path planning module is used to plan a global path from the vehicle position to the target point according to the vehicle position information and the position and map information of the target point, the local path planning module is used to plan according to a reference line, map information, vehicle position information, and obstacle information using a factor graph method, and output the trajectory of the vehicle in the next few seconds, the reference line refers to the path between a first preset distance in front of the vehicle and a second preset distance behind the vehicle, and the control module is used to calculate the acceleration and the turning angle based on the trajectory in the next few seconds given by the local planning module and the vehicle position information, and send them to the vehicle chassis or a simulator.

[0009] A motion planning method in an uncertain environment based on a factor graph is performed by a motion planning system in an uncertain environment based on a factor graph, and the method comprises:

[0010] Step S410, obtaining obstacle information through a perception module;

[0011] Step S420, providing vehicle location information through a map and positioning module;

[0012] Step S430, planning a global path from the vehicle position to the target point according to the vehicle position information, the target point position and map information through the global path planning module;

[0013] Step S440, a local path planning module is used to plan based on a reference line, map information, vehicle position information, and obstacle information using a factor graph method, and outputs a trajectory of the vehicle in the next few seconds. The reference line refers to a path between a first preset distance in front of the vehicle and a second preset distance behind the vehicle.

[0014] Step S450, the control module calculates the acceleration and turning angle based on the trajectory of the next few seconds given by the local planning module and the vehicle position information, and sends them to the vehicle chassis or simulator.

[0015] Beneficial effects:

[0016] Efficient handling of uncertainty: The present invention introduces a factor graph-based motion planning algorithm, which can handle the perceived and predicted uncertainties in the autonomous driving system in real time, especially the dynamic behaviors of other traffic participants and their uncertainties, thereby generating a safer and more reasonable driving trajectory in a complex traffic environment.

[0017] Improve the safety of trajectory planning: Traditional motion planning algorithms often ignore or simplify uncertainty factors, which can easily lead to unsafe trajectory planning and increase the risk of collision. However, the present invention can provide a safer planning solution by comprehensively considering uncertainty factors, effectively avoiding the risks caused by perception errors or inaccurate predictions.

[0018] Reduce computing resource consumption: Compared with other methods, the present invention adopts a more efficient factor graph optimization algorithm when dealing with uncertainty, which greatly reduces the required computing resources and operation time, can meet real-time requirements, and is suitable for efficient trajectory planning of autonomous driving. BRIEF DESCRIPTION OF THE DRAWINGS

[0019] Figure 1 A schematic block diagram of a motion planning system in an uncertain environment based on a factor graph is shown.

[0020] Figure 2 A schematic illustration showing a path planning factor graph.

[0021] Figure 3 A schematic illustration showing a speed planning factor graph.

[0022] Figure 4 A schematic flowchart of a motion planning method in an uncertain environment based on a factor graph is shown. DETAILED DESCRIPTION

[0023] The exemplary embodiments of the present disclosure will be described in more detail below with reference to the accompanying drawings. Although the exemplary embodiments of the present disclosure are shown in the accompanying drawings, it should be understood that the present disclosure can be implemented in various forms, and the present disclosure should not be limited by the embodiments described herein. On the contrary, these embodiments are provided to enable a more thorough understanding of the present disclosure and to fully convey the scope of the present disclosure to those skilled in the art.

[0024] Figure 1 Figure 2 shows a schematic block diagram of a motion planning system in an uncertain environment based on a factor graph. Figure 1 As shown, the motion planning system in an uncertain environment based on factor graph is divided into multiple modules, including a perception module 110, a map and positioning module 120, a global path planning module 130, a local path planning module 140 and a control module 150, as shown in FIG. Figure 1As shown. Among them, the perception module 110 is used to obtain obstacle information, the map and positioning module 120 is used to provide the vehicle position information, the global path planning module 130 is used to plan a global path from the vehicle position to the target point according to the vehicle position information and the position and map information of the target point, the local path planning module 140 is used to plan according to the reference line, map information, vehicle position information, obstacle information, and the factor graph method, and output the trajectory of the vehicle in the next few seconds. The reference line refers to the path between the first preset distance in front of the vehicle and the second preset distance behind the vehicle. The control module 150 is used to calculate the acceleration and the turning angle based on the trajectory of the next few seconds given by the local path planning module 140 and the vehicle position information, and send them to the vehicle chassis or simulator.

[0025] The implementation of each module is described in detail below.

[0026] Perception module 110:

[0027] The perception module consists of a 3D target detection module and a 3D multi-target tracking module. The input of the 3D target detection module is the radar point cloud, and the output is the position of the i-th obstacle. , the 3D multi-target tracking module is used to match and track each obstacle and obtain the position of the i-th obstacle ,speed The covariance matrix of position, velocity and uncertainty is represented by Gaussian noise. , The variables are The variance of is the covariance between variables. The perception frequency of the perception module is 10 Hz, and the state information of obstacles is updated every 0.1 second.

[0028] Map and Positioning Module 120:

[0029] The map and positioning module provides information such as the vehicle's location, lane lines, traffic signs, and road boundaries, and supports global and local path planning. The map and positioning module provides the vehicle's precise location, with an update frequency of 10Hz.

[0030] Global path planning module 130:

[0031] According to the vehicle position information, the target point position and map information, A The algorithm plans a global path from the vehicle's position to the target point, and extracts the path 100 meters in front of the vehicle and 20 meters behind the vehicle as the reference line for the local planning of the vehicle. The global path planning frequency is 10Hz. The algorithms are commonly used path finding and graph traversal algorithms in this field.

[0032] Local path planning module 140:

[0033] The local path planning module uses the factor graph method to plan based on the reference line, map information, vehicle position information, obstacle information, etc., and outputs the trajectory of the vehicle in the next 4 seconds. The local path planning frequency is 10Hz.

[0034] The local path planning module can include an obstacle handling module, a path planning module, a speed planning module, and a trajectory fusion module. They are introduced one by one below.

[0035] Obstacle handling module:

[0036] The position of the obstacle from the perception module is in the natural coordinate system, while the local planning performs motion planning in the curvilinear coordinate system (Frenet coordinate system), so coordinate transformation is required: for the obstacle position, the projection method is used for transformation, and for the covariance matrix, the unscented Kalman filter method is used for coordinate change.

[0037] Path planning module:

[0038] In the path planning module, planning is performed in the sl space: the location of obstacles, curvature, and vehicle dynamics factors are considered to design corresponding constraints, and they are converted into probability factors in the factor graph. The optimal path is optimized through the path planning factor graph optimization algorithm. During optimization, s (longitudinal distance) is sampled at equal intervals with a spacing of 5m, and l (lateral distance) is optimized using the factor graph.

[0039] The path planning factor graph is as follows Figure 2 As shown. Figure 2 As shown in Figure 1, the factors used in the path planning factor graph optimization algorithm include:

[0040] Static obstacle collision factor: Utilizes the position uncertainty of the obstacle to calculate the collision probability between the ego vehicle and the obstacle in the SL space to ensure the safety of the trajectory.

[0041] Curvature Factor: Based on the vehicle’s position, the curvature of the trajectory is calculated to ensure that the trajectory complies with the vehicle’s physical steering capabilities.

[0042] Dynamic factor: According to the vehicle's dynamic model, limit the speed, acceleration, etc. to make them conform to the vehicle's kinematic characteristics.

[0043] Prior factor: represents the current lateral state of the vehicle, ensuring that the value of the first variable after optimization is as consistent as possible with the current state.

[0044] In the initial optimization, the dynamic factor, static obstacle collision factor, and curvature constraint factor are considered, and the optimization function is:

[0045] ,

[0046] is the horizontal distance, is the vertical distance, is a lateral state vector representing the lateral path, , Indicates the horizontal distance The vertical distance The function of is the first-order derivative of the lateral distance with respect to the longitudinal distance, is the second-order derivative of the lateral distance with respect to the longitudinal distance, is the reference value of the initial lateral state variable, is a kernel function, The whole is used to represent the kinetic factors and the prior factors.

[0047] represents the collision probability with static obstacles on the path, is the penalty weight matrix associated with static obstacles, Used to represent the static obstacle collision factor.

[0048] represents the curvature of the path, is the curvature-dependent penalty weight matrix, Used to represent the curvature constraint factor.

[0049] Indicates finding variables Make the following expression get the minimum value, It represents the final optimized lateral state vector after considering the above factors.

[0050] Speed ​​planning module:

[0051] In the speed planning module, planning is performed in the st space: the speed of dynamic obstacles, the reference speed of the vehicle, vehicle dynamics and other factors are considered to design the corresponding constraints, and they are converted into probability factors in the factor graph, and the optimal speed is optimized through the speed planning factor graph optimization algorithm. During the optimization, t (time) is sampled at equal intervals with an interval of 0.2s, and s (longitudinal distance) is optimized using the factor graph.

[0052] Speed ​​planning factor diagram Figure 3 As shown. Figure 3 As shown in Figure 2, the factors in the speed planning factor graph optimization algorithm include:

[0053] Dynamic obstacle collision factor: Utilize the speed uncertainty of the obstacle to calculate the collision probability between the vehicle and the obstacle in the st space to ensure the safety of the trajectory.

[0054] Reference speed factor: Based on the reference speed given by the decision, make the vehicle speed as similar to the decision speed as possible.

[0055] Dynamic factor: According to the vehicle's dynamic model, limit the speed, acceleration, etc. to make them conform to the vehicle's kinematic characteristics.

[0056] Prior factor: represents the current longitudinal state of the vehicle, ensuring that the value of the first variable after optimization is as consistent as possible with the current state;

[0057] During the initial optimization, the dynamic factor, dynamic obstacle collision factor, and reference speed factor are considered, and the optimization function is:

[0058] ,

[0059] is the vertical distance, It's time. is a longitudinal state vector, used to represent the longitudinal path, , is the vertical distance With time The function of is the first-order derivative of the longitudinal distance with respect to time, is the second-order derivative of the longitudinal distance with respect to the time distance, is the reference value of the initial longitudinal state variable, is a kernel function, The whole is used to represent the kinetic factors and the prior factors.

[0060] represents the collision probability with dynamic obstacles, is the penalty weight matrix associated with dynamic obstacles, Used to represent the dynamic obstacle collision factor.

[0061] Indicates the similarity to the expected reference speed, is the reference speed-related penalty weight matrix, Used to indicate the reference speed factor.

[0062] Indicates finding variables Make the following expression get the minimum value, It represents the final optimized longitudinal state vector after considering the above factors.

[0063] Trajectory fusion module:

[0064] The trajectory fusion module is used to fuse the results of the path planning module and the speed planning module to obtain a three-dimensional trajectory, and transform it into a natural coordinate system as the final trajectory, and send it to the control module 150.

[0065] Control module 150:

[0066] It is used to calculate the acceleration and turning angle based on the trajectory of the next 4 seconds given by the local planning module 140 and the vehicle position information, and send them to the vehicle chassis or simulator. The tracking frequency of the control module 150 is 100 Hz.

[0067] In summary, an autonomous driving navigation framework that takes uncertainty into account is provided: in the perception module, the positions and uncertainties of different vehicles are obtained, and this uncertainty is taken into account in local planning and processed during factor graph optimization.

[0068] Local planning algorithm considering the uncertainty of other participants: In order to deal with the uncertainty of autonomous driving perception, the present invention designs a motion planning algorithm based on factor graph to deal with the uncertainty of other traffic participants. The method first tracks the perception results to obtain their uncertainty. Then, in motion planning, the uncertainty corresponding to the obstacles is considered horizontally and vertically respectively, and a horizontal and vertical trajectory is given, and finally a three-dimensional trajectory is given.

[0069] Efficient and versatile motion planning algorithm: By using the factor graph algorithm to perform trajectory planning, the fast optimization speed and accurate results of the factor graph method can be utilized to quickly give the optimal trajectory.

[0070] According to the present invention, a method for motion planning in an uncertain environment based on a factor graph is also provided, which is executed by a motion planning system in an uncertain environment based on a factor graph. Figure 4 As shown, the method includes:

[0071] Step S410, obtaining obstacle information through the perception module 110;

[0072] Step S420, providing the vehicle location information through the map and positioning module 120;

[0073] Step S430, planning a global path from the vehicle position to the target point according to the vehicle position information and the target point position and map information through the global path planning module 130;

[0074] Step S440, the local path planning module 140 uses a factor graph method to plan based on the reference line, map information, vehicle position information, and obstacle information, and outputs the trajectory of the vehicle in the next few seconds. The reference line refers to the path between a first preset distance in front of the vehicle and a second preset distance behind the vehicle.

[0075] Step S450, the control module 150 calculates the acceleration and the turning angle based on the trajectory of the next several seconds given by the local planning module and the vehicle position information, and sends them to the vehicle chassis or simulator.

[0076] In the description provided herein, a large number of specific details are described. However, it is understood that embodiments of the present invention can be practiced without these specific details. In some instances, well-known methods, structures and techniques are not shown in detail so as not to obscure the understanding of this description.

[0077] Although the present invention has been described according to a limited number of embodiments, it will be apparent to those skilled in the art, with the benefit of the above description, that other embodiments may be envisioned within the scope of the invention thus described. In addition, it should be noted that the language used in this specification is primarily selected for readability and instructional purposes, rather than for the purpose of explaining or limiting the subject matter of the present invention.

Claims

1. A motion planning system in an uncertain environment based on factor graph, characterized in that: The system comprises a perception module (110), a map and positioning module (120), a global path planning module (130), a local path planning module (140) and a control module (150), wherein the perception module (110) is used to obtain obstacle information, the map and positioning module (120) is used to provide vehicle position information, the global path planning module (130) is used to plan a global path from the vehicle position to the target point based on the vehicle position information and the position and map information of the target point, the local path planning module (140) is used to plan based on a reference line, map information, vehicle position information and obstacle information using a factor graph method, and output a trajectory of the vehicle in the next several seconds, wherein the reference line refers to a path between a first preset distance in front of the vehicle and a second preset distance behind the vehicle, and the control module (150) is used to calculate the acceleration and the turning angle based on the vehicle position information and the trajectory in the next several seconds given by the local planning module (140), and send the calculations to the vehicle chassis or a simulator, The local path planning module includes obstacle handling module, path planning module, speed planning module and trajectory fusion module: The obstacle processing module is used to transform the obstacle position using the projection method and to change the coordinates of the covariance matrix using the unscented Kalman filter method; The path planning module is used for planning in the SL space: considering the location of obstacles, curvature, and vehicle dynamics factors to design corresponding constraints, and converting them into probability factors in the factor graph, and optimizing the optimal path through the path planning factor graph optimization algorithm; The speed planning module is used for planning in the st space: considering the speed of dynamic obstacles, reference speed, and vehicle dynamics factors to design corresponding constraints, and converting them into probability factors in the factor graph, and optimizing the optimal speed through the speed planning factor graph optimization algorithm; A trajectory fusion module, used to fuse the results of the path planning module and the speed planning module to obtain a three-dimensional trajectory, and transform it into a natural coordinate system as the final trajectory, and send it to the control module (150); The factors used in the path planning factor graph optimization algorithm include: Static obstacle collision factor: using the position uncertainty of the obstacle, the collision probability between the ego vehicle and the obstacle in the sl space is calculated to ensure the safety of the trajectory; Curvature factor: Based on the vehicle’s position, the curvature of the trajectory is calculated to ensure that the trajectory complies with the vehicle’s physical steering capabilities; Dynamic factor: According to the vehicle's dynamic model, limit the speed, acceleration, etc. to make it conform to the vehicle's kinematic characteristics; Prior factor: represents the current lateral state of the vehicle, ensuring that the value of the first variable after optimization is as consistent as possible with the current state; In the initial optimization, the priori factor, dynamic factor, static obstacle collision factor, and curvature constraint factor are considered, and the optimization function is: ， is the horizontal distance, is the vertical distance, is a lateral state vector representing the lateral path, , Indicates the horizontal distance The vertical distance The function of is the first-order derivative of the lateral distance with respect to the longitudinal distance, is the second-order derivative of the lateral distance with respect to the longitudinal distance, is the reference value of the initial lateral state variable, is a kernel function, The whole is used to represent the kinetic and a priori factors; represents the collision probability with static obstacles on the path, is the penalty weight matrix associated with static obstacles, Used to represent the static obstacle collision factor; represents the curvature of the path, is the curvature-dependent penalty weight matrix, Used to represent the curvature constraint factor; Indicates finding variables Make the following expression get the minimum value, It represents the final optimized lateral state vector after considering the above factors.

2. The motion planning system in an uncertain environment based on factor graph according to claim 1, characterized in that: The perception module (110) consists of a 3D target detection module and a 3D multi-target tracking module. The input of the 3D target detection module is the radar point cloud, and the output is the position of the i-th obstacle. , the 3D multi-target tracking module is used to match and track each obstacle and obtain the position of the i-th obstacle ,speed The covariance matrix of position, velocity and uncertainty is represented by Gaussian noise. , The variables are The variance of is the covariance between the variables.

3. The motion planning system in an uncertain environment based on factor graph according to claim 2, characterized in that: The global path planning module (130) uses A The algorithm plans a global path from the vehicle's position to the target point, and extracts the path 100 meters in front of the vehicle and 20 meters behind the vehicle as a reference line for the vehicle's local planning.

4. The motion planning system in an uncertain environment based on factor graph according to claim 3, characterized in that: The local path planning module (140) uses a factor graph method to plan based on the reference line, map information, vehicle position information, and obstacle information, and outputs the trajectory of the vehicle in the next 4 seconds.

5. The motion planning system in an uncertain environment based on factor graph according to claim 4, characterized in that: The control module (150) is used to calculate the acceleration and the turning angle based on the trajectory of the next 4 seconds given by the local planning module (140) and the position information of the vehicle, and send them to the vehicle chassis or the simulator.

6. The motion planning system in an uncertain environment based on factor graph according to claim 4, characterized in that: When optimizing, the path planning module performs evenly spaced sampling of s, i.e., longitudinal distance, with a spacing of 5m, and uses a factor graph to optimize l, i.e., lateral distance. When optimizing, the speed planning module performs evenly spaced sampling of t, i.e., time, with a spacing of 0.2s, and uses a factor graph to optimize s, i.e., longitudinal distance.

7. The motion planning system in an uncertain environment based on factor graph according to claim 4, characterized in that: The factors in the speed planning factor graph optimization algorithm include: Dynamic obstacle collision factor: Using the uncertainty of the obstacle’s speed, the collision probability between the vehicle and the obstacle in the st space is calculated to ensure the safety of the trajectory; Reference speed factor: according to the reference speed given by the decision, make the vehicle speed as similar as possible to the decision speed; Dynamic factor: According to the vehicle's dynamic model, limit the speed, acceleration, etc. to make it conform to the vehicle's kinematic characteristics; Prior factor: represents the current longitudinal state of the vehicle, ensuring that the value of the first variable after optimization is as consistent as possible with the current state; During the initial optimization, the dynamic factor, dynamic obstacle collision factor, and reference speed factor are considered, and the optimization function is: ， is the vertical distance, It's time. is a longitudinal state vector, used to represent the longitudinal path, , is the vertical distance With time The function of is the first-order derivative of the longitudinal distance with respect to time, is the second-order derivative of the longitudinal distance with respect to the time distance, is the reference value of the initial longitudinal state variable, is a kernel function, The whole is used to represent the kinetic and a priori factors; represents the collision probability with dynamic obstacles, is the penalty weight matrix associated with dynamic obstacles, Used to represent the dynamic obstacle collision factor; Indicates the similarity to the expected reference speed, is the reference speed-related penalty weight matrix, Used to indicate the reference speed factor; Indicates finding variables Make the following expression get the minimum value, It represents the final optimized longitudinal state vector after considering the above factors.

8. A method for motion planning in an uncertain environment based on a factor graph, performed by a motion planning system in an uncertain environment based on a factor graph according to any one of claims 1 to 7, characterized in that: Methods include: Step S410, obtaining obstacle information through the perception module (110); Step S420, providing the vehicle location information through the map and positioning module (120); Step S430, planning a global path from the vehicle position to the target point according to the vehicle position information and the target point position and map information through the global path planning module (130); Step S440, using the local path planning module (140) to plan based on the reference line, map information, vehicle position information, and obstacle information, using a factor graph method to output the trajectory of the vehicle in the next few seconds, where the reference line refers to a path between a first preset distance in front of the vehicle and a second preset distance behind the vehicle; Step S450, the control module (150) calculates the acceleration and the turning angle based on the trajectory of the next several seconds given by the local planning module (140) and the position information of the vehicle, and sends the calculation result to the vehicle chassis or the simulator; The local path planning module includes obstacle handling module, path planning module, speed planning module and trajectory fusion module: The obstacle processing module is used to transform the obstacle position using the projection method and to change the coordinates of the covariance matrix using the unscented Kalman filter method; The path planning module is used for planning in the SL space: considering the location of obstacles, curvature, and vehicle dynamics factors to design corresponding constraints, and converting them into probability factors in the factor graph, and optimizing the optimal path through the path planning factor graph optimization algorithm; The speed planning module is used for planning in the st space: considering the speed of dynamic obstacles, reference speed, and vehicle dynamics factors to design corresponding constraints, and converting them into probability factors in the factor graph, and optimizing the optimal speed through the speed planning factor graph optimization algorithm; A trajectory fusion module, used to fuse the results of the path planning module and the speed planning module to obtain a three-dimensional trajectory, and transform it into a natural coordinate system as the final trajectory, and send it to the control module (150); The factors used in the path planning factor graph optimization algorithm include: Static obstacle collision factor: using the position uncertainty of the obstacle, the collision probability between the ego vehicle and the obstacle in the sl space is calculated to ensure the safety of the trajectory; Curvature factor: Based on the vehicle’s position, the curvature of the trajectory is calculated to ensure that the trajectory complies with the vehicle’s physical steering capabilities; Dynamic factor: According to the vehicle's dynamic model, limit the speed, acceleration, etc. to make it conform to the vehicle's kinematic characteristics; Prior factor: represents the current lateral state of the vehicle, ensuring that the value of the first variable after optimization is as consistent as possible with the current state; In the initial optimization, the priori factor, dynamic factor, static obstacle collision factor, and curvature constraint factor are considered, and the optimization function is: ， is the horizontal distance, is the vertical distance, is a lateral state vector representing the lateral path, , Indicates the horizontal distance The vertical distance The function of is the first-order derivative of the lateral distance with respect to the longitudinal distance, is the second-order derivative of the lateral distance with respect to the longitudinal distance, is the reference value of the initial lateral state variable, is a kernel function, The whole is used to represent the kinetic and a priori factors; represents the collision probability with static obstacles on the path, is the penalty weight matrix associated with static obstacles, Used to represent the static obstacle collision factor; represents the curvature of the path, is the curvature-dependent penalty weight matrix, Used to represent the curvature constraint factor; Indicates finding variables Make the following expression get the minimum value, It represents the final optimized lateral state vector after considering the above factors.

Images