Automatic driving path planning and tracking control method

Abstract

The invention discloses an automatic driving path planning and tracking control method, which belongs to the technical field of automatic driving and comprises the following steps of: 1, generating a global path and a tracking path according to a current position and a planned target point in a vehicle driving process; 2, constructing a local cost map by adopting a multi-sensor fusion method, carrying out local path planning, and generating a vehicle real-time obstacle avoidance path; 3, evaluating and selecting a tracking path, an obstacle avoidance path and a lane keeping and changing path, and deciding an optimal local path; 4, performing transverse / longitudinal control on the vehicle by using a path tracking transverse / longitudinal control system; the method has the advantages that the tracking path conforming to the kinematics law can be planned for the vehicle, the vehicle can autonomously run according to the set speed and path according to the expected path output by the planning module and under the cooperation of the integrated navigation system, and the problems that in the prior art, single vehicle path planning and tracking control cannot be achieved are solved.

Description

Technical Field

[0001] The present invention belongs to the technical field of autonomous driving, and in particular relates to an autonomous driving path planning and tracking control method. Background Art

[0002] The Motor Vehicle Auto Driving System, also known as autonomous vehicles (Self-piloting automobile), is also called unmanned vehicles, computer-driven vehicles, or wheeled mobile robots. It is an intelligent vehicle system that achieves unmanned driving through an on-board computer system. At present, in order to ensure the safety of autonomous driving technology, autonomous vehicles are usually equipped with a variety of sensors, such as cameras, millimeter-wave radars, inertial measurement units, etc. However, autonomous vehicles inevitably have blind spots due to weather, obstacles, light, etc. The limited sensors on autonomous vehicles (AVs) alone cannot meet the requirements of autonomous driving technology for real-time observation of dynamic environments. Blind spots pose a great challenge to the reliability of path planning for autonomous vehicles. Vehicle-road collaboration technology uses sensors of roadside units and nearby vehicles to provide autonomous vehicles with sensor information of their blind spots. In an autonomous driving system based on vehicle-road collaboration, roadside units (RSUs) and AVs work together to extend the line of sight of autonomous vehicles. The collaborative perception information is calculated and processed, and the results are returned to the TAV through vehicle-to-vehicle (V2V) communication and vehicle-to-instructure (V2I) communication, providing more comprehensive and complete information for TAV's path planning, greatly improving the safety of autonomous driving.

[0003] During the driving process of an autonomous vehicle, for road scenes including intersections, in addition to collaborative planning of vehicles through multi-vehicle collaborative control, single-vehicle path planning is also an important module of autonomous vehicles. Single-vehicle path planning requires searching for a safe, collision-free path based on a given vehicle starting point and target point on an environmental map using a planning algorithm; however, in the prior art, no method has been designed that can perform single-vehicle path planning and tracking control, which has made the implementation of single-vehicle path planning and tracking control a problem that needs to be solved urgently in this field.

[0004] Therefore, the present invention proposes an automatic driving path planning and tracking control method to solve the problems existing in the above-mentioned prior art. Summary of the invention

[0005] 1. Technical issues to be resolved

[0006] In view of the shortcomings of the prior art, the present invention provides an automatic driving path planning and tracking control method, which has the advantages of being able to plan a tracking path for a vehicle that conforms to the laws of kinematics, and according to the desired path output by the planning module, with the cooperation of the combined navigation system, the vehicle can achieve autonomous driving at a set speed and path, etc., solving the problem that the prior art cannot achieve single-vehicle path planning and tracking control, etc.

[0007] (II) Technical solution

[0008] In order to achieve the above-mentioned purpose of being able to plan a tracking path that conforms to the kinematic law for the vehicle and enable the vehicle to drive autonomously according to the set speed and path, the present invention provides the following technical solutions:

[0009] An automatic driving path planning and tracking control method, comprising:

[0010] Step 1: During the driving process of the vehicle, according to the current position output by the positioning module and the planned target point, a global path is generated in the global map based on the Dijkstra algorithm, and the vehicle generates a tracking path based on the global path and the global map;

[0011] Step 2: For the tracking path given based on the global path obtained in step 1, a local cost map is constructed in real time using a multi-sensor fusion method, and local path planning based on the DWA algorithm is performed in the local cost map to generate a real-time obstacle avoidance path for the vehicle;

[0012] Step 3: Generate lane keeping and lane changing paths based on the information obtained by the vehicle vision system, evaluate and select the tracking path, obstacle avoidance path, lane keeping and lane changing path, and decide on the optimal local path;

[0013] Step 4: For the optimal local path obtained in step 3, with the cooperation of the integrated navigation system, the path tracking lateral and longitudinal control system uses a closed-loop control algorithm to control the vehicle laterally / longitudinally, so that the vehicle can drive autonomously according to the set speed and path.

[0014] According to a specific implementation of the embodiment of the present disclosure, a global path is generated based on the Dijkstra algorithm, and a process in which a vehicle generates a tracking path based on the global path and the global map includes:

[0015] Step 1.1: In the global map, initialize the distance of the starting point to 0 and the distance of other points to ∞;

[0016] Step 1.2: Create a set open to store the node currently being processed, and add the starting point to open;

[0017] Step 1.3: Create a set Q to store all unprocessed points;

[0018] Step 1.4: When the Q set is not empty, repeat the following loop: find the point with the minimum distance in the open, update the distance values ​​of the points around this point, add them to the open, and remove the current point from the open; after the end, obtain the shortest distance to the target point and the corresponding path sequence.

[0019] According to a specific implementation of the embodiment of the present disclosure, the local cost map construction process includes:

[0020] (1) Given the vehicle pose sequence and observation sequence, solve the local environment map. Assuming that each grid is independent and identically distributed, the problem can be converted into solving the occupancy probability of each grid;

[0021] (2) By setting the threshold of grid occupancy probability, occupied grids, unknown grids and idle grids are distinguished;

[0022] (3) Every time a frame of sensor data is obtained, the status of the corresponding grid is updated in real time, and weights are assigned to the grid area of ​​the obstacle area and the grid area around the obstacle to obtain a cost map. Finally, the cost map is fused with the global map containing obstacle information using the map fusion method.

[0023] According to a specific implementation method of an embodiment of the present disclosure, the process of local path planning based on the DWA algorithm is to sample multiple groups of speed combinations from the speed space (v, ω) composed of linear velocity and angular velocity, and predict the movement of the vehicle through the vehicle kinematic model to obtain multiple groups of motion trajectories of the vehicle within a specified time, and then evaluate these trajectories through an evaluation function, select the optimal trajectory, and generate a real-time obstacle avoidance path.

[0024] According to a specific implementation of the embodiment of the present disclosure, when evaluating and selecting the tracking path, obstacle avoidance path, lane keeping and lane changing path, the path cost function used is:

[0025] C total =kC obs +C s

[0026] Among them, C total is the total cost of the trajectory, k is the proportional coefficient, C obs For the safety cost, C s is the smoothness cost of the trajectory;

[0027] C s =Δβ∫v 2 ds

[0028] Where: Δβ is the change in heading angle, v is the current speed of the vehicle, and s is the trajectory.

[0029] According to a specific implementation of the embodiment of the present disclosure, the decision process of the optimal local path includes:

[0030] Firstly, the path cost function is used to evaluate the costs of obstacle avoidance path, tracking path, lane keeping and lane changing path respectively, and the path with the minimum cost is selected as the optimal path.

[0031] According to a specific implementation of the embodiment of the present disclosure, the path tracking lateral and longitudinal control system uses PID control as a controller for lateral / longitudinal control of the vehicle, and the differential equation of the PID controller is:

[0032] u(t)=K P e(t)+K I ∫e(t)dt+K D e(t)

[0033] Among them, K P , K I , K D are the proportional coefficient, integral coefficient, and differential coefficient respectively, e(t) is the error signal under feedback, and u(t) is the control signal obtained after the superposition of proportional, integral, and differential links;

[0034] Discretize the PID differential equation and get the calculation formula of the PID control algorithm:

[0035]

[0036] Among them, k is a number, indicating a certain sampling time point, k-1 indicates the one before k, i is a variable between 0 and k, T is the controller sampling time, T I =TK P / K I ,T D =K D / K P .

[0037] According to a specific implementation of the embodiment of the present disclosure, the path tracking lateral and longitudinal control system includes:

[0038] Longitudinal desired speed decision and speed tracking PID longitudinal speed tracking control model based on road curvature, used for longitudinal speed decision and longitudinal speed tracking of autonomous driving vehicles;

[0039] A path-tracking feedback control model based on dynamic preview is used for lateral path tracking of autonomous vehicles.

[0040] According to a specific implementation of the embodiment of the present disclosure, the process of using the longitudinal desired speed decision and speed tracking PID longitudinal speed tracking control model based on road curvature to make longitudinal speed decision and longitudinal speed tracking of the autonomous driving vehicle includes:

[0041] The vehicle longitudinal desired speed decision based on road curvature is used, and the vehicle longitudinal desired speed is tracked and controlled by a PID controller. When the PID controller obtains the longitudinal desired speed and the actual vehicle speed, the speed deviation is obtained, and the PID control algorithm is applied to calculate the control amount of the accelerator pedal and the brake pedal, thereby realizing the longitudinal speed decision and longitudinal speed tracking of the autonomous driving vehicle.

[0042] According to a specific implementation of the embodiment of the present disclosure, the process of performing lateral path tracking of an autonomous driving vehicle using a path tracking feedback control model based on dynamic preview includes:

[0043] Firstly, the preview distance is determined by utilizing the preview distance that continuously changes based on the desired path curvature. Then, the preview heading angle deviation is calculated at high vehicle speeds, and the final feedback error is calculated by combining the lateral distance deviation and the preview heading angle deviation at low vehicle speeds. The feedback error is controlled through the PID control algorithm, and the final desired steering wheel angle is output to achieve lateral path tracking of the autonomous driving vehicle.

[0044] (III) Beneficial effects

[0045] Compared with the prior art, the present invention provides an automatic driving path planning and tracking control method, which has the following beneficial effects:

[0046] 1. This method adopts a global-to-local focusing approach, combines multi-sensor fusion and the global path generated by the Dijkstra algorithm to build a local map, and then generates a real-time obstacle avoidance path based on the local path planning of DWA, which can accurately calculate the optimal path solution from the starting point to the end point;

[0047] 2. This method generates lane keeping and lane changing paths based on lane line information obtained by the visual system. It also evaluates and selects the three paths based on the local cost map and decides on the optimal local path, which improves the accuracy of planning and enhances the vehicle's adaptability to dynamic scenes, facilitating timely obstacle avoidance.

[0048] 3. When this method tracks and controls the vehicle, if the vehicle speed is too fast on a path with large curvature during driving, a good path tracking effect cannot be achieved. Therefore, the lateral control is combined with the longitudinal control to effectively improve the trajectory tracking effect of the autonomous driving vehicle while ensuring stability. BRIEF DESCRIPTION OF THE DRAWINGS

[0049] Figure 1This is a flow chart of the automatic driving path planning and tracking control method of the present invention;

[0050] Figure 2 It is the starting point diagram of the path planning of the Dijkstra algorithm of the present invention;

[0051] Figure 3 The end point graph of the Dijkstra algorithm path planning of the present invention;

[0052] Figure 4 This is a schematic diagram of the current optimal path of the present invention;

[0053] Figure 5 This is a diagram of the optimal path settlement process when there are three obstacles in the present invention;

[0054] Figure 6 It is a local path planning framework diagram of the present invention;

[0055] Figure 7 It is a schematic diagram of map fusion and cost map of the present invention;

[0056] Figure 8 The radar of the car of the present invention scans the global map;

[0057] Fig. 9 The radar of the car of the present invention scans the local map;

[0058] Fig.10 This is a schematic diagram of the local planning of DWA of the present invention;

[0059] Fig.11 It is the experimental route map of the present invention;

[0060] Fig.12 It is a schematic diagram of the expected path of the present invention;

[0061] Fig.13 This is the PID principle diagram of the present invention;

[0062] Fig.14 Schematic diagram of the longitudinal and lateral control system of the autonomous driving vehicle of the present invention

[0063] Among them: Fig.11 In the figure, Figure (a) is a simulation diagram of the optimal path for the smart car; Figure (b) is a fitting curve diagram of the path coordinates. DETAILED DESCRIPTION

[0064] The following will be combined with the drawings in the embodiments of the present invention to clearly and completely describe the technical solutions in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0065] The technical solutions provided by various embodiments of the present invention are described in detail below in conjunction with the accompanying drawings.

[0066] See also Figure 1-Figure 14 , Figure 1 One of the flowcharts of the automatic driving path planning and tracking control method provided by one embodiment of the present invention is as follows Figure 1 As shown, this method is used for path planning under automatic driving, so that the vehicle has obstacle avoidance capability, and the method includes the following steps:

[0067] Step 1: During the driving process of the vehicle, according to the current position output by the positioning module and the planned target point, a global path is generated in the global map based on the Dijkstra algorithm, and the vehicle generates a tracking path based on the global path and the global map;

[0068] Step 2: For the tracking path given based on the global path obtained in step 1, a multi-sensor fusion method is used to construct a local cost map in real time, and local path planning based on the DWA algorithm is performed in the local cost map to generate a real-time obstacle avoidance path for the vehicle;

[0069] Step 3: Generate lane keeping and lane changing paths based on lane line information obtained by the vehicle vision system; finally, evaluate and select the three paths based on the local cost map to determine the optimal local path;

[0070] Step 4: For the optimal local path obtained in step 3, with the cooperation of the integrated navigation system, the path tracking lateral and longitudinal control system adopts a closed-loop control algorithm to control the vehicle lateral / longitudinal, and outputs control instructions to the wire-controlled chassis, so that the vehicle can drive autonomously according to the set speed and path.

[0071] In the embodiment of the present invention, in the process of generating a global path based on the Dijkstra algorithm in the global map described in step 1, a breadth-first search strategy is adopted to search and expand from the starting point to the surrounding areas at the same time, and the path with the minimum weight is preferentially selected until the end point is reached; the specific process includes:

[0072] Step 1.1: In the global map, first initialize the distance of the starting point to 0 and the distances of other points to ∞;

[0073] Step 1.2: Create a set open to store the node currently being processed, and add the starting point to open;

[0074] Step 1.3: Create a set Q to store all unprocessed points;

[0075] Step 1.4: When the Q set is not empty, repeat the following loop: find the point with the minimum distance in open, update the distance values ​​of the points around this point, add them to open, and remove the current point from open; after the algorithm is completed, the shortest distance to the target point and the corresponding path sequence can be obtained.

[0076] The process of generating a global path based on the Dijkstra algorithm as described in the above embodiment is as follows: Figure 2 , Figure 3 , Figure 4 and Figure 5 As shown, the specific algorithm flow is as follows:

[0077] First, define the grid map. The point in the upper left corner is (0, 0). The horizontal direction is the X axis, from 0-800, and the vertical direction is the Y axis, from 0-600. Each grid is a square with a side length of 20. The starting point of the Dijkstra algorithm path planning Figure 2 S is the starting point, at which the obstacle level is 0 and there is only one obstacle; the end point of the Dijkstra algorithm path planning Figure 3 G is the end point set. The process will start from the set starting point and calculate all points of the traversal path to obtain the optimal path solution; the current optimal path diagram Figure 4 The solution of the optimal path after calculation and all calculated path points. The red dots in the numbers represent the calculated path points. The green path from S to G is the optimal path.

[0078] It can be seen that the method described in step 1 can accurately calculate the optimal path solution from the starting point to the end point. It can be seen that the color of the grid from the starting point to the end point is getting darker, which is linked to the number in the grid. Next, increase the obstacle level and verify the process of Dijkstra algorithm. Figure 5 This is the optimal path settlement process when there are three obstacles.

[0079] In the embodiment of the present invention, Figure 6 As shown in FIG. 2 , the process of performing local path planning based on DWA in the local cost map described in step 2 includes two stages: the stage of establishing a real-time environment map containing obstacle information and the stage of generating trajectories using the DWA algorithm. According to the position given by the positioning module and the local cost map constructed in real time, the vehicle uses the DWA algorithm to perform local path planning, and finally evaluates and makes decisions based on multiple types of paths.

[0080] The process of establishing a real-time environment map containing obstacle information is to use a multi-sensor fusion algorithm to build an environment model using the grid method, divide the workspace into uniform grids, and each grid represents the obstacle information detected here, and establish an occupancy grid map (Occupancy Grid Map) in the vehicle coordinate system for the autonomous driving scene. The specific process of building a local cost map is as follows:

[0081] (1) Given the vehicle pose sequence and observation sequence, the local environment map is solved. Assuming that each grid is independent and identically distributed, the problem can be converted into solving the occupancy probability of each grid; (2) By setting the threshold of the grid occupancy probability, occupied grids, unknown grids and idle grids are distinguished; (3) Every time a frame of sensor data is obtained, the status of the corresponding grid is updated in real time. A certain weight is assigned to the grid in the obstacle area and the grid area within a certain range around the obstacle, and a cost map can be obtained. Finally, this cost map is used for local planning.

[0082] In order to improve the accuracy of planning, this embodiment also adopts a map fusion method to fuse the obstacle map and the semantic map for planning. Figure 7 As shown in the figure: The Static map layer is the global map, that is, the grid map of the entire scene. Some properties of the grid map, including length, width, resolution, etc., are pre-set. The LiDAR layer uses real-time laser information and current posture estimation to build a map. The map of this layer needs to be fused with the global map in advance to update the global map. This also enhances the vehicle's adaptability to dynamic scenes and timely avoids obstacles. Figure 8 It is a two-dimensional global map presented by the car's radar scanning. Fig. 9 It is a local map, where the black ones are boundaries, the spots are obstacles, and the red ones are obstacles detected by the radar in real time.

[0083] In this embodiment, the main idea of ​​using the DWA algorithm for local path planning is to sample multiple groups of speed combinations in the speed space (v, ω) composed of linear velocity and angular velocity, and predict the movement of the vehicle through the vehicle kinematic model to obtain multiple groups of motion trajectories of the vehicle within a specified time, and then evaluate these trajectories through an evaluation function to select the optimal trajectory.

[0084] like Fig.10 As shown in the figure, a car model is used for modeling, and geometric constraints of the car body are added to the speed sampling space, including speed constraints to limit the maximum speed of the vehicle; acceleration constraints: limit the maximum acceleration and maximum deceleration of the vehicle; the same applies to angular velocity and angular acceleration; limit the maximum left and right turning angles of the steering wheel. Through these geometric constraints, the speed sampling space is limited to a feasible dynamic range. The motion state of the vehicle can be predicted by the following formula:

[0085]

[0086] θ t =θ t +wΔt

[0087] Then generate a real-time obstacle avoidance path.

[0088] In the embodiment of the present invention, Fig.11 As shown in Figure 3, the process of evaluating and selecting the three paths based on the local cost map and deciding the optimal local path is to combine the multiple paths obtained previously and select an optimal path for the vehicle. First, the cost of the obstacle avoidance path, tracking path, and lane keeping and lane changing path must be evaluated separately, and then the path with the lowest cost is selected for execution.

[0089] In order to ensure that the vehicle makes the path as smooth as possible while ensuring safety and ensuring smoothness during driving, the following path cost function formula is proposed:

[0090] C totat =kC obs +C s

[0091] In the formula, C total is the total cost of the trajectory, k is the proportional coefficient, C obs For the safety cost, C s is the smoothness cost of the trajectory.

[0092] By projecting the trajectory onto the local cost map, the cost sum of all grids traversed by the trajectory is taken as the safety cost C of the trajectory. obs .

[0093] The trajectory is evaluated using evaluation methods such as the distance to the obstacle, the difference in position and posture with the target point, the curvature of the generated trajectory, and the speed change. An optimal local path is selected by minimizing the evaluation cost function.

[0094] The smoothness of the path needs to take into account the current speed of the vehicle and the change in heading angle. To avoid the influence of the speed direction, the square of the speed is used in the formula, and the product of the two is integrated along the trajectory, that is,

[0095] C s =Δβ∫v 2 ds

[0096] Where: Δβ is the change in heading angle, v is the current speed of the vehicle, and s is the trajectory.

[0097] Based on the above path cost function, the path with the minimum cost is selected as the optimal path, and the planned path is sent to the vehicle control module with path points. The experimental path is obtained as follows Fig.11 As shown, Figure (a) is a simulation diagram of the optimal path for the smart car to travel; Figure (b) is a fitting curve diagram of the path coordinates.

[0098] In the embodiment of the present invention, in step 4, a reference path description is first required. The reference path used in this embodiment is composed of a series of equally spaced reference waypoints. The distance between every two reference waypoints is set to 10 cm. Each reference waypoint exists in the form of an array, and the information contained therein includes the x, y coordinates of the waypoint in the global coordinate system, and the heading angle. like Fig.12 As shown in Table 1, the reference path is oriented toward the angle The vehicle's orientation angle when the driver is driving to collect reference waypoints. If this embodiment needs to complete some special tasks, it can also be achieved by adding semantic information to the waypoints; when tracking the path, only the reference waypoints need to be judged to obtain the information contained in the current waypoint.

[0099] Table 1: Expected path description

[0100]

[0101] It is worth noting that in actual operation, you need to first select the origin of a global coordinate system and determine the positive direction of the x-axis and y-axis of the global coordinate system; then the latitude and longitude coordinates of the reference waypoints and the vehicle's real-time position can be converted into x, y coordinates in the global coordinate system.

[0102] After the reference path is described, the automatic driving vehicle trajectory tracking control is then performed. The path tracking lateral and longitudinal control system of this embodiment adopts PID control to control the longitudinal speed and lateral steering angle respectively. The PID control principle is as follows: Fig.13 shown in Fig.13 In the equation, r(t) is the expected signal, e(t) is the error signal under feedback, and the control signal u(t) is obtained after the superposition of the proportional, integral, and differential links. The output signal of the controlled object after receiving the control signal is y(t); thus, the differential equation of PID control is:

[0103] u(t)=K P e(t)+K I ∫e(t)dt+K D e(t)

[0104] Among them, K P , K I , K D They are proportional coefficient, integral coefficient and differential coefficient respectively;

[0105] By discretizing the PID differential equation, the calculation formula of the PID control algorithm can be obtained as follows:

[0106]

[0107] Among them, k is a number, indicating a certain sampling time point, k-1 indicates the one before k, i is a variable between 0 and k, T is the controller sampling time, T I =TK P / K I ,T D =K D / K P .

[0108] Based on the above theory, this embodiment applies PID control as the controller for longitudinal speed tracking and lateral path tracking of the autonomous driving vehicle.

[0109] In order to meet the accuracy of the path tracking control system and the real-time performance and stability of the controller, this embodiment designs a path tracking lateral and longitudinal control system, which is mainly divided into two parts. The first part is the longitudinal desired speed decision based on the road curvature and the speed tracking PID longitudinal speed tracking control model, which is used for the longitudinal control of the autonomous driving vehicle. The second part is the path tracking feedback control model based on dynamic preview, which is used for the lateral path tracking of the autonomous driving vehicle. The detailed lateral and longitudinal control system of the autonomous driving vehicle is as follows Fig.14 shown.

[0110] The path tracking lateral and longitudinal control system uses a closed-loop control algorithm to control the vehicle lateral / longitudinal. The specific process includes controlling the vehicle longitudinally and transversely, among which:

[0111] For the longitudinal control of the vehicle, it can be divided into two parts, namely the longitudinal speed decision of the vehicle and the longitudinal speed tracking. The present embodiment adopts the longitudinal speed decision of the vehicle based on the road curvature, and tracks the longitudinal desired speed of the vehicle through the PID controller. When the controller obtains the longitudinal desired speed and the actual vehicle speed, the speed deviation can be obtained, and the PID control algorithm is applied to calculate the control amount of the accelerator pedal and the brake pedal, thereby realizing the vehicle speed control.

[0112] For lateral control, this embodiment designs a lateral path tracking feedback control model based on the preview algorithm in the feedback linearization method in the field of path tracking and Stanley based on distance and heading angle errors. First, by using the preview distance based on the continuously changing expected path curvature to determine the preview distance in the preview model, the problem of large tracking error and easy bending caused by the traditional preview distance that is linearly related to the vehicle speed or a fixed value is solved. Subsequently, by using the preview heading angle error model at high vehicle speeds and combining the lateral distance error and the preview heading angle error at low vehicle speeds, the final feedback error is calculated, and the feedback error is controlled by PID control to output the final expected steering wheel angle, thereby improving the trajectory tracking effect of the autonomous driving vehicle while ensuring stability.

[0113] It should be noted that, in this article, relational terms such as first and second, etc. are only used to distinguish one entity or operation from another entity or operation, and do not necessarily require or imply any such actual relationship or order between these entities or operations. Moreover, the terms "include", "comprise" or any other variants thereof are intended to cover non-exclusive inclusion, so that a process, method, article or device including a series of elements includes not only those elements, but also other elements not explicitly listed, or also includes elements inherent to such process, method, article or device. In the absence of further restrictions, the elements defined by the sentence "comprise a ..." do not exclude the existence of other identical elements in the process, method, article or device including the elements.

[0114] Although embodiments of the present invention have been shown and described, it will be appreciated by those skilled in the art that various changes, modifications, substitutions and variations may be made to the embodiments without departing from the principles and spirit of the present invention, and that the scope of the present invention is defined by the appended claims and their equivalents.

Claims

1. An automatic driving path planning and tracking control method, characterized in that: include Step 1: During the driving process of the vehicle, according to the current position output by the positioning module and the planned target point, a global path is generated in the global map based on the Dijkstra algorithm, and the vehicle generates a tracking path based on the global path and the global map; Step 2: For the tracking path given based on the global path obtained in step 1, a local cost map is constructed in real time using a multi-sensor fusion method, and local path planning based on the DWA algorithm is performed in the local cost map to generate a real-time obstacle avoidance path for the vehicle; Step 3: Generate lane keeping and lane changing paths based on the information obtained by the vehicle vision system, evaluate and select the tracking path, obstacle avoidance path, lane keeping and lane changing path, and decide on the optimal local path; Step 4: For the optimal local path obtained in step 3, with the cooperation of the integrated navigation system, the path tracking lateral and longitudinal control system uses a closed-loop control algorithm to control the vehicle laterally / longitudinally, so that the vehicle can drive autonomously according to the set speed and path.

2. The method for automatic driving path planning and tracking control according to claim 1, characterized in that: The process of generating a global path based on the Dijkstra algorithm and the vehicle generating a tracking path based on the global path and the global map includes: Step 1.1: In the global map, initialize the distance of the starting point to 0 and the distance of other points to ∞; Step 1.2: Create a set open to store the node currently being processed, and add the starting point to open; Step 1.3: Create a set Q to store all unprocessed points; Step 1.4: When the Q set is not empty, repeat the following loop: find the point with the minimum distance in the open, update the distance values ​​of the points around this point, add them to the open, and remove the current point from the open; after the end, obtain the shortest distance to the target point and the corresponding path sequence.

3. The automatic driving path planning and tracking control method according to claim 1, characterized in that: The local cost map construction process includes: (1) Given the vehicle pose sequence and observation sequence, solve the local environment map. Assuming that each grid is independent and identically distributed, the problem is converted into solving the occupancy probability of each grid. (2) By setting the threshold of grid occupancy probability, occupied grids, unknown grids and idle grids are distinguished; (3) Every time a frame of sensor data is obtained, the status of the corresponding grid is updated in real time, and weights are assigned to the grid area of ​​the obstacle area and the grid area around the obstacle to obtain a cost map. Finally, the cost map is fused with the global map containing obstacle information using the map fusion method.

4. The method for automatic driving path planning and tracking control according to claim 1, characterized in that: The process of local path planning based on the DWA algorithm is to sample multiple groups of speed combinations from the speed space (v, ω) composed of linear velocity and angular velocity, and predict the movement of the vehicle through the vehicle kinematic model to obtain multiple groups of motion trajectories of the vehicle within a specified time. These trajectories are then evaluated through the evaluation function, the optimal trajectory is selected, and a real-time obstacle avoidance path is generated.

5. The method for automatic driving path planning and tracking control according to claim 1, characterized in that: When evaluating and selecting the tracking path, obstacle avoidance path, lane keeping and lane changing path, the path cost function used is: C total =kC obs +C s Among them, C total is the total cost of the trajectory, k is the proportional coefficient, C obs For security cost, C s is the smoothness cost of the trajectory; C s =Δβ∫v 2 ds Where: Δβ is the change in heading angle, v is the current speed of the vehicle, and s is the trajectory.

6. The method for automatic driving path planning and tracking control according to claim 5, characterized in that: The decision process of the optimal local path includes: Firstly, the path cost function is used to evaluate the costs of obstacle avoidance path, tracking path, lane keeping and lane changing path respectively, and the path with the minimum cost is selected as the optimal path.

7. The method for automatic driving path planning and tracking control according to claim 1, characterized in that: The path tracking lateral and longitudinal control system uses PID control as the controller for lateral / longitudinal control of the vehicle respectively. The differential equation of the PID controller is: u(t)=K P e(t)+K I ∫e(t)dt+K D e(t) Among them, K P , K I , K D are the proportional coefficient, integral coefficient, and differential coefficient respectively, e(t) is the error signal under feedback, and u(t) is the control signal obtained after the superposition of proportional, integral, and differential links; Discretize the PID differential equation and get the calculation formula of the PID control algorithm: Among them, k is a number, indicating a certain sampling time point, k-1 indicates the one before k, i is a variable between 0 and k, T is the controller sampling time, T I =TK P / K I , T D =K D / K P .

8. The method for automatic driving path planning and tracking control according to claim 7, characterized in that: The path-following lateral and longitudinal control system includes: Longitudinal desired speed decision and speed tracking PID longitudinal speed tracking control model based on road curvature, used for longitudinal speed decision and longitudinal speed tracking of autonomous driving vehicles; A path-tracking feedback control model based on dynamic preview is used for lateral path tracking of autonomous vehicles.

9. The method for automatic driving path planning and tracking control according to claim 8, characterized in that: The process of making longitudinal speed decision and longitudinal speed tracking of an autonomous driving vehicle using the longitudinal desired speed decision and speed tracking PID longitudinal speed tracking control model based on road curvature includes: The vehicle longitudinal desired speed decision based on road curvature is used, and the vehicle longitudinal desired speed is tracked and controlled by a PID controller. When the PID controller obtains the longitudinal desired speed and the actual vehicle speed, the speed deviation is obtained, and the PID control algorithm is applied to calculate the control amount of the accelerator pedal and the brake pedal, thereby realizing the longitudinal speed decision and longitudinal speed tracking of the autonomous driving vehicle.

10. The method for automatic driving path planning and tracking control according to claim 8, characterized in that: The process of lateral path tracking of an autonomous vehicle using a dynamic preview-based path tracking feedback control model includes: Firstly, the preview distance is determined by utilizing the preview distance that continuously changes based on the desired path curvature. Then, the preview heading angle deviation is calculated at high vehicle speeds, and the final feedback error is calculated by combining the lateral distance deviation and the preview heading angle deviation at low vehicle speeds. The feedback error is controlled through the PID control algorithm, and the final desired steering wheel angle is output to achieve lateral path tracking of the autonomous driving vehicle.

Images