Vehicle formation driving control method based on improved DWA algorithm

Abstract

The present application provides a vehicle formation driving control method based on an improved DWA algorithm, comprising the following steps: S1, establishing a vehicle formation, where the vehicle formation includes at least one leading vehicle, and each leading vehicle includes at least one following vehicle; S2, the leading vehicle continuously drives according to the leading mode, and each following vehicle continuously drives according to the formation following mode until the vehicle formation reaches the target point and ends driving, or any vehicle in the vehicle formation enters the mode switching range and enters step S3; S3, the vehicles that do not enter the mode switching range keep their driving modes unchanged, and the vehicles that enter the mode switching range continuously plan paths and drive according to the improved DWA algorithm until they drive out of the mode switching range and return to step S2. The control method provided by the present application can effectively improve the obstacle avoidance efficiency of intelligent vehicles and ensure timely return to the original driving trajectory after obstacle avoidance.

Description

Technical Field

[0001] This application belongs to the technical field of vehicle intelligent control, and provides a vehicle formation driving control method based on an improved DWA algorithm. Background Art

[0002] The Dynamic Window Approach (DWA) is a real-time algorithm for path planning and obstacle avoidance of intelligent vehicles. By comprehensively considering environmental constraints such as the speed and acceleration limits of intelligent vehicles and obstacles, an evaluation function is established to finally obtain the optimal driving path.

[0003] Although the traditional DWA algorithm has been widely used, there are still some problems that need to be continuously improved and perfected, mainly manifested in: (1) During the obstacle avoidance process of intelligent vehicles, the moving distance is too large, resulting in a decrease in the driving efficiency of intelligent vehicles; (2) After the obstacle avoidance of intelligent vehicles, there is a possibility of not being able to return to the original driving trajectory, which may lead to traffic paralysis and even traffic accidents in actual driving situations. Summary of the Invention

[0004] To solve the problems existing in the above-mentioned prior art, this application provides a vehicle formation driving control method based on an improved DWA algorithm. The control method includes the following steps:

[0005] S1. Establish a vehicle formation, where the vehicle formation includes at least one leading vehicle, and each leading vehicle includes at least one following vehicle;

[0006] S2. The leading vehicle continuously drives in the leading mode, and each following vehicle continuously drives in the formation following mode until the vehicle formation reaches the target point and ends driving, or any vehicle in the vehicle formation drives into the mode switching range and enters step S3;

[0007] S3. Vehicles that have not entered the mode switching range keep their driving modes unchanged, and vehicles that have entered the mode switching range continuously plan paths and drive according to the improved DWA algorithm until they drive out of the mode switching range and return to step S2.

[0008] Further, when any vehicle in the vehicle formation drives into the mode switching range, the distance between it and the obstacle is less than the mode switching threshold D shown in the following formula S :

[0009]

[0010] where D min is the shortest distance for this arbitrary vehicle to plan a path to avoid the obstacle, D maxis the maximum distance for the arbitrary vehicle to maintain the formation in the vehicle formation, v max is the maximum linear speed of the arbitrary vehicle, v t is the real-time linear speed of the arbitrary vehicle, and a is an exponential constant.

[0011] Further, the vehicle that enters the mode switching range in step S3 continuously plans the path and travels according to the improved DWA algorithm until it exits the mode switching range and returns to step S2, which specifically includes the following steps:

[0012] S31, sample the linear speed v and angular speed w of the vehicle that enters the mode switching range;

[0013] S32, determine each optional sampling speed combination based on the speed constraint conditions, and each sampling speed combination includes a group of optional linear speed v and angular speed w;

[0014] S33, simulate the driving trajectories corresponding to each optional sampling speed combination;

[0015] S34, determine the optimal driving trajectory based on the improved DWA evaluation function G(v, w) and control the vehicle in the mode switching range to travel along the optimal driving trajectory;

[0016] S35, judge whether the end point of the optimal driving trajectory has exited the mode switching range. If so, execute step S2. If not, return to execute step S31.

[0017] Further, the improved DWA evaluation function G(v, w) is specifically:

[0018]

[0019] where dist_goal(v, w) is the target point distance evaluation function, dist(v, w) is the obstacle distance evaluation function, velocity(v, w) is the speed evaluation function, ideal_way_goal(v, w) is the trajectory fitting degree evaluation function, α, β, γ, η are weight coefficients, and δ() is the normalization operation.

[0020] Preferably, the trajectory fitting degree evaluation function ideal_way_goal(v, w) is determined by the following formula:

[0021] ideal_way_goal(v, w) = 1 / Max[Distance(Path1, Path2)],

[0022] Among them, Path1 is the driving trajectory obtained by simulating the sampling speed combination, Path2 is the driving trajectory determined based on global planning, and Distance(Path1, Path2) is the distance between corresponding nodes on the two trajectories.

[0023] Preferably, the leading vehicle in step S2 continuously drives in the leading mode, specifically: the leading vehicle continuously drives along the driving trajectory determined by the A* algorithm or the artificial potential field method.

[0024] Preferably, the following vehicle in step S2 continuously drives in the formation following mode, specifically by cyclically executing the following steps:

[0025] S21, sample the position, attitude, linear velocity, and angular velocity of the following vehicle j and the leading vehicle i it follows at the current moment;

[0026] S22, determine the control quantities of the following vehicle j at the next moment based on the leader-following method, where the control quantities are the linear velocity and angular velocity of the vehicle;

[0027] S23, respectively construct the relative distance potential field and the relative angle potential field

[0028] S24, respectively based on and determine the potential field force of the relative distance and the potential field force of the relative angle between the leading vehicle i and the following vehicle j to obtain the total potential field force between the leading vehicle i and the following vehicle j

[0029] S25, correct the control quantities of the following vehicle j by adjusting the total potential field force ;

[0030] S26, control the following vehicle j based on the corrected control quantities.

[0031] A vehicle formation driving control method based on an improved DWA algorithm provided by an embodiment of the present application improves the traditional DWA algorithm. By replacing the azimuth evaluation function with the distance evaluation function from the current position of the vehicle to the target position, it effectively solves the problem of excessive moving distance during the obstacle avoidance process of intelligent vehicles. Further, by introducing the trajectory fitting degree evaluation function, it avoids the problem that the vehicle cannot return to the original driving trajectory after obstacle avoidance, thereby effectively improving the efficiency and safety of vehicle formation driving. Description of the Drawings

[0032] Figure 1It is a schematic diagram of the implementation process of an existing DWA algorithm;

[0033] Figure 2 It is a schematic diagram of the effect of a vehicle using an existing DWA algorithm to avoid obstacles;

[0034] Figure 3 It is a flowchart of a vehicle formation driving control method based on an improved DWA algorithm provided by an embodiment of the present application;

[0035] Figure 4 It is a schematic diagram of the process of a following vehicle driving in a formation following mode in some embodiments of the present application;

[0036] Figure 5 It is a schematic diagram of determining a mode switching range in some embodiments of the present application;

[0037] Figure 6 It is a schematic diagram of the implementation process of a vehicle entering a mode switching range continuously planning a path and driving according to an improved DWA algorithm in some embodiments of the present application;

[0038] Figure 7 It is a schematic diagram of the principle of determining a trajectory fitting evaluation function in some embodiments of the present application;

[0039] Figure 8a It is a schematic diagram of the vehicle formation situation in the first stage of the driving process in a specific embodiment of the present application;

[0040] Figure 8b It is a schematic diagram of the vehicle formation situation in the second stage of the driving process in a specific embodiment of the present application;

[0041] Figure 8c It is a schematic diagram of the vehicle formation situation in the third stage of the driving process in a specific embodiment of the present application;

[0042] Figure 8d It is a schematic diagram of the vehicle formation situation in the fourth stage of the driving process in a specific embodiment of the present application;

[0043] Figure 8e It is a schematic diagram of the vehicle formation situation in the fifth stage of the driving process in a specific embodiment of the present application;

[0044] Figure 8f It is a schematic diagram of the vehicle formation situation in the sixth stage of the driving process in a specific embodiment of the present application;

[0045] Figure 8g It is a schematic diagram of the vehicle formation situation in the seventh stage of the driving process in a specific embodiment of the present application;

[0046] Figure 8hSchematic diagram of vehicle formation in the 8th stage of the driving process in a specific embodiment of the present application;

[0047] Figure 8i Schematic diagram of vehicle formation in the 9th stage of the driving process in a specific embodiment of the present application;

[0048] Figure 8j Schematic diagram of vehicle formation in the 10th stage of the driving process in a specific embodiment of the present application;

[0049] Figure 9 Schematic diagram of the optimal global path in a specific embodiment of the present application;

[0050] Figure 10a Schematic diagram of the turning path planned by the vehicle formation at the wall in a specific embodiment of the present application;

[0051] Figure 10b Schematic diagram of the path planned by the vehicle formation to avoid the first obstacle in a specific embodiment of the present application;

[0052] Figure 10c Schematic diagram of the path planned by the vehicle formation to avoid the second obstacle in a specific embodiment of the present application;

[0053] Figure 10d Schematic diagram of the path planned by the vehicle formation before reaching the target point in a specific embodiment of the present application. Detailed implementation manners

[0054] Hereinafter, the present application will be further described based on preferred implementation manners with reference to the accompanying drawings.

[0055] [Conventional DWA algorithm]

[0056] As described in the background art, the Dynamic Window Approach (DWA) is a real-time algorithm for path planning and obstacle avoidance of intelligent vehicles. Figure 1 shows a flowchart of an implementation of a common DWA algorithm, as Figure 1 shown, the basic principle of the DWA algorithm for local path planning of an intelligent vehicle is to collect the vehicle driving speed (including linear speed v and angular speed w) through the sensors of the intelligent vehicle, and all the speed combinations (v, w) collected constitute a speed space.

[0057] Furthermore, during the speed sampling process, due to the differences in the vehicle's own performance (including speed limits and acceleration limits) and road environment (such as the safe distance from obstacles), the speed sampling is restricted by constraints, resulting in a certain range of optional speed combinations, which is also called the speed sampling window. Speed sampling is performed within the above-mentioned sampling window in the speed space. For each group of speed combinations (v, w) obtained by sampling, trajectory simulation is carried out, and each trajectory is evaluated through an evaluation function to select the optimal trajectory for local path planning. Since the vehicle is constantly moving and the data collected by the sensor is also constantly changing, and the window size is also constantly changing, this algorithm is also called the Dynamic Window Approach (DWA) algorithm.

[0058] In the DWA algorithm, the evaluation function is used to evaluate the trajectories simulated according to each group of speed combinations (v, w), so as to select the optimal trajectory for local path planning of the intelligent vehicle. A classic evaluation function is shown as follows:

[0059] G(v, w) = δ(α·heading(v, w) + β·dist(v, w) + γ·velocity(v, w)),

[0060] In the above formula, heading(v, w), dist(v, w), and velocity(v, w) are the azimuth evaluation function, distance evaluation function, and speed evaluation function respectively.

[0061] The azimuth evaluation function is used to evaluate the magnitude of the azimuth angle between the end point of the driving trajectory of the intelligent vehicle simulated under the current speed combination and the direction of the target point. The larger the azimuth angle, the smaller the function value, and vice versa. When the vehicle reaches the target point, the azimuth angle is 0 and the function value reaches the maximum. The purpose of the azimuth evaluation function is to eliminate the trajectories that deviate from the direction of the target point and select the trajectory with the smallest azimuth angle to the target point. In some specific embodiments, the expression of the azimuth evaluation function is:

[0062]

[0063] where the azimuth angle θ offset is the angle by which the vehicle's driving direction deviates from the direction of the target point.

[0064] The distance evaluation function is used to evaluate the distance between the current position of the intelligent vehicle and the nearest obstacle. The smaller the distance, the smaller the function value.

[0065] The speed evaluation function is used to evaluate the magnitude of the vehicle speed. The greater the vehicle's driving speed, the greater the function value. When the vehicle speed reaches the maximum speed, the function value reaches the maximum. In some specific embodiments, the expression of the distance evaluation function is:

[0066]

[0067] Among them, v max is the maximum speed that the vehicle can reach in the speed space.

[0068] The above evaluation functions are respectively assigned corresponding weights α, β, and γ, and normalized (represented by the normalization operator δ), and finally the overall evaluation function G(v, w) is obtained.

[0069] Although the conventional DWA algorithm has been widely used, the following problems still exist in the actual path planning process:

[0070] (1) When the smart car is avoiding obstacles, the distance it moves is too large, which results in a decrease in the efficiency of the smart car.

[0071] When driving, smart vehicles will perform obstacle avoidance operations to avoid obstacles. However, under the conventional DWA algorithm, smart vehicles will avoid obstacles with larger movements (such as Figure 2 This causes the vehicle to waste a lot of time in the obstacle avoidance process, resulting in a decrease in driving efficiency.

[0072] (2) After the smart car completes obstacle avoidance, it cannot return to its original trajectory, which may cause traffic paralysis or even traffic accidents in actual situations.

[0073] After the smart vehicle has finished avoiding obstacles, it needs to re-plan the trajectory to drive to the target point. In most cases, it will not follow the original trajectory. This situation needs to be handled more carefully for smart vehicles traveling on the road. This is because the road itself has a certain width. If the smart vehicle arbitrarily plans the path after the obstacle avoidance, it will cause road traffic disorder, traffic congestion, and reduce road transportation efficiency. In serious cases, traffic accidents will occur, causing casualties and property losses. Therefore, it is necessary to make the smart car return to the original trajectory as soon as possible after the obstacle avoidance is completed.

[0074] [Technical framework of this application]

[0075] In order to solve the above problems existing in the prior art, the present application improves the conventional DWA algorithm through embodiments and applies it to the control of vehicle platoon driving.

[0076] Figure 3 A flow chart of a vehicle platoon driving control method based on an improved DWA algorithm provided in accordance with an embodiment of the present application is shown, as Figure 3 As shown, the control method includes the following steps:

[0077] S1, establishing a vehicle formation, wherein the vehicle formation includes at least one pilot vehicle, and each pilot vehicle includes at least one following vehicle;

[0078] S2. The leading vehicle continuously travels in the leading mode, and each of the following vehicles continuously travels in the formation following mode until the vehicle formation reaches the target point and ends the travel, or any vehicle in the vehicle formation enters the mode switching range and enters step S3;

[0079] S3. Vehicles that have not entered the mode switching range keep their driving modes unchanged, and vehicles that enter the mode switching range continuously plan paths and travel according to the improved DWA algorithm until they drive out of the mode switching range and return to step S2.

[0080] As Figure 3 shown, during the implementation process of this control method, first, at least one leading vehicle and following vehicles following the leading vehicle are set through step S1 to form a vehicle formation; the leading vehicle and the following vehicles continuously travel according to their respective driving modes in step S2 until they reach the target point, or at least one vehicle enters the mode switching range and enters step S3. In step S3, the vehicles that enter the mode switching range perform path planning and travel according to the improved DWA algorithm (or are said to travel in the obstacle avoidance mode), and other vehicles keep their respective driving modes until the vehicles in the obstacle avoidance mode drive out of the mode switching range. At this time, the vehicle formation returns to step S2 and resumes traveling according to the original driving mode.

[0081] The following details the specific implementation manners of the above steps.

[0082] [Vehicle Formation and Normal Driving Mode]

[0083] In step S1, the formation of the vehicle formation can use formations known to those skilled in the art, such as triangular, diamond-shaped, and single-file formations. Among them, each vehicle formation includes at least one leading vehicle and at least one following vehicle following the leading vehicle.

[0084] After the vehicle formation is completed, it can enter step S2 and start traveling from the starting point to the target point. In the embodiments of the present application, for the control of the vehicle formation, a distributed control method is adopted. The leading vehicle and each following vehicle travel according to the preset driving modes respectively. During the travel, data such as position, attitude, and speed are collected in real time through sensors provided on their respective vehicles, and interactive communication is carried out through communication methods such as wireless Internet of Things.

[0085] Specifically, the driving mode adopted by the leading vehicle is called the leading mode, and the leading mode can adopt various path planning and navigation methods known to those skilled in the art. For example, in some preferred embodiments, the A* algorithm or the artificial potential field method can be used to determine the driving path of the leading vehicle.

[0086] The driving mode of each following vehicle is called the platoon following mode, and the goal or constraint of this mode is to enable each following vehicle to achieve good tracking of its leading vehicle. In some preferred embodiments, such as Figure 4 shown, the platoon following mode adopted by each following vehicle specifically includes continuously performing the following steps:

[0087] Step S21: Sample the position, attitude, linear velocity, and angular velocity of following vehicle j and its leading vehicle i at the current moment respectively.

[0088] Specifically, for each vehicle, the displacement between its rotation center and the reference point is D. At any moment, the position of leading vehicle i can be represented by the position (x i , y i ) of its rotation center, or the position (r xi , r yi ) of the reference point. Its linear velocity, angular velocity, and angle of attack are represented by v i , w i , θ i respectively. The above position coordinates, velocity, and angle information reflect the running state of leading vehicle i at this moment. The above running state can be obtained in real time by various sensors installed on the leading vehicle and shared with each following vehicle through communication systems such as wireless Internet of Things.

[0089] Similarly, (x j , y j ), (r xj , r yj ), as well as v j , w j , θ j and other running state information can be obtained in real time by various sensors installed on each following vehicle j and shared with leading vehicle i through communication systems such as wireless Internet of Things. Correspondingly, after obtaining the running states of the leading vehicle and the following vehicle, the distance l and relative angle between the leading vehicle and the following vehicle can be determined

[0090] Step S22: Determine the control quantities of following vehicle j at the next moment based on the leader-follower method. The control quantities are the linear velocity and angular velocity of the vehicle.

[0091] The leader-follower method is a method for controlling vehicle cooperative platooning, that is, by controlling the linear velocity and / or angular velocity of the following vehicle to keep its formation with the leading vehicle. Among them, the control rate based on is a relatively commonly used control rate of the leader-follower method.

[0092] Specifically, in the embodiments of the present application, the following can be adopted The control rate controls each following vehicle j to track the pilot vehicle i:

[0093]

[0094] Among them, l, are the relative distance and relative angle between the leading vehicle i and the following vehicle j, respectively, l d , are the ideal relative distance and ideal relative angle between the leading vehicle i and the following vehicle j, respectively; D is the displacement between the rotation center of the following vehicle j and the reference point; v i 、w i ,θ i are the linear velocity, angular velocity and angle of attack of the pilot vehicle i, v j 、w j ,θ j are the linear velocity, angular velocity and angle of attack of the following vehicle j respectively, and α1 and α2 are the proportional control coefficients.

[0095] Step S23: construct the relative distance potential field between the pilot vehicle i and the following vehicle j respectively and relative angle potential field

[0096] Step S24, based on and The potential force that determines the relative distance between the lead vehicle i and the following vehicle j and the potential force at relative angles Get the total potential field force between the leading vehicle i and the following vehicle j

[0097] Step S25, by adjusting the total potential field force Correct the control amount of the following vehicle j;

[0098] S26: Control the following vehicle j based on the corrected control amount.

[0099] The pilot following method can achieve better coordinated formation control during the stable driving of the pilot vehicle. However, when the pilot vehicle is driving, due to various emergencies, its linear velocity, angular velocity and other operating states suddenly change in a short period of time, the trajectory deviation of the formation vehicles is likely to occur, or the formation formation may become divergent or convergent. In severe cases, it may even lead to the collapse of the formation. For this reason, in a preferred embodiment of the present application, the conventional pilot following method is corrected through steps S23 to S25 to improve the problem of poor stability of the existing formation method in the face of emergencies.

[0100] Specifically, and As shown in the following formula:

[0101]

[0102] By introducing the above relative distance potential field function and relative angle potential field function, the following vehicle j is not only restricted by the conventional leader-follower control law, but also additionally affected by the potential field force caused by the changes of l and Furthermore, by adjusting l and in the relative distance potential field function and relative angle potential field function, the control quantity of the following vehicle j at the next moment can be corrected, so that it can better track the leading vehicle i.

[0103] In addition to correcting the control quantity of the following vehicle j through the above steps, when the motion state of the leading vehicle changes suddenly, the formation of the cooperative formation may also be distorted and stretched, resulting in the possibility of collision between the following vehicles. Therefore, in some preferred embodiments, the control quantity of each following vehicle j can be further corrected to avoid collision between them.

[0104] Specifically, first, an anti-collision repulsive force potential field between each following vehicle j of the leading vehicle i can be constructed based on the following formula

[0105]

[0106] where d is the distance between two following vehicles, d0 is the safe distance between following vehicles, τ is the gain coefficient of the anti-collision repulsive force potential field, and μ is the distance gain coefficient;

[0107] Then, the anti-collision repulsive force between each following vehicle j of the leading vehicle i can be determined based on the following formula

[0108]

[0109] Finally, by adjusting the anti-collision repulsive force the control quantity of each following vehicle j is corrected.

[0110] [Switching of Driving Modes and Improved DWA Algorithm]

[0111] During the driving process of each intelligent vehicle in the vehicle formation, path planning is carried out throughout the whole process. A large amount of calculation and data are required in the perception and decision-making process. Therefore, the requirement for computing power is extremely high, and there may be situations where the planned trajectory is not optimal or even collides with obstacles due to data overload and high planning frequency.

[0112] Therefore, in the embodiments of the present application, the normal driving process is carried out through step S2. Only after a vehicle in the vehicle formation approaches an obstacle and enters the mode switching range, the driving mode is switched and it is made to perform path planning and driving according to the improved DWA algorithm through step S3.

[0113] Specifically, as Figure 5 shown, the mode switching range is a circular area where the distance from the obstacle is less than the mode switching threshold D S . The setting of the mode switching threshold D S cannot be too large, otherwise all vehicles need to operate in the obstacle avoidance mode during the entire formation driving process, which will greatly consume computing resources; the setting of D S cannot be too small either, otherwise the vehicle formation cannot perform obstacle avoidance processing in time, resulting in collision accidents. Therefore, in some preferred embodiments, for any vehicle, its mode switching threshold D S is determined by the following formula:

[0114]

[0115] where D min is the shortest distance for the arbitrary vehicle to plan a path to avoid the obstacle, D max is the farthest distance for the arbitrary vehicle to maintain the formation in the vehicle formation, v max is the maximum linear velocity of the arbitrary vehicle, v t is the real-time linear velocity of the arbitrary vehicle, and a is an exponential constant, generally greater than or equal to 1. It can be seen from the above formula that when v t approaches v max infinitely, D S also approaches D max infinitely. This enables sufficient distance to be left for obstacle avoidance operations even when the vehicle speed is very high, so that the driving of the vehicle formation can balance efficiency and safety.

[0116] In some embodiments, the vehicle that enters the mode switching range continuously plans the path and drives according to the improved DWA algorithm until it exits the mode switching range and returns to step S2. As Figure 6 shown, its specific steps include:

[0117] S31, sampling the linear velocity v and angular velocity w of the vehicle that enters the mode switching range;

[0118] S32, determining each optional sampling speed combination based on the speed constraint conditions, and each sampling speed combination includes a group of optional linear velocity v and angular velocity w;

[0119] S33, simulating the driving trajectories corresponding to each optional sampling speed combination;

[0120] S34. Determine the optimal driving trajectory based on the improved DWA evaluation function G(v, w) and control the vehicle within the mode switching range to drive along the optimal driving trajectory;

[0121] S35. Determine whether the end point of the optimal driving trajectory has exited the mode switching range. If so, execute step S2; if not, return to execute step S31.

[0122] The implementation manners of the above steps S31 to S33 are the same as those of the existing DWA algorithm and will not be elaborated here.

[0123] Step S34 uses the improved DWA evaluation function G(v, w) to evaluate each driving trajectory, and its specific form is:

[0124]

[0125] where dist_goal(v, w) is the target point distance evaluation function, dist(v, w) is the obstacle distance evaluation function, velocity(v, w) is the speed evaluation function, ideal_way_goal(v, w) is the trajectory fitting degree evaluation function, α, β, γ, η are weight coefficients, and δ() is the normalization operation.

[0126] The difference between the above evaluation function and the conventional DWA algorithm is that the azimuth angle evaluation function is replaced by the target point distance evaluation function, and at the same time, the trajectory fitting degree evaluation function is added.

[0127] The target point distance evaluation function dist_goal(v, w) is used to evaluate the distance between the vehicle and the target point. The farther the distance, the lower the evaluation value; conversely, the higher. Using the target point distance evaluation function instead of the azimuth angle evaluation function can make the vehicle drive towards the target point faster, thereby effectively improving the driving efficiency.

[0128] The trajectory fitting degree evaluation function ideal_way_goal(v, w) is mainly used to evaluate the deviation degree between the driving trajectory obtained by simulation and the global path. The smaller the deviation degree, the higher the evaluation value.

[0129] As Figure 7 shown, in some preferred embodiments, the trajectory fitting degree evaluation function ideal_way_goal(v, w) is determined by the following formula:

[0130] ideal_way_goal(v, w) = 1 / Max[Distance(Path1, Path2)],

[0131] Among them, Path1 is the driving trajectory obtained by simulating the sampling speed combination, Path2 is the driving trajectory determined based on global planning, and Distance(Path1, Path2) is the distance between corresponding nodes on the two trajectories. [Specific Embodiment]

[0133] In this embodiment, the driving of the vehicle formation is simulated by the above vehicle formation driving control method based on the improved DWA algorithm. In this embodiment, the obstacle positions are (5, 0, 0), (6, 3, 0), (5, 6, 0) respectively, the target point position is (6, 6, 0), and there are also two walls in the driving area.

[0134] Figures 8a to 8j The vehicle formation situations at 8 stages during the driving process are shown in sequence. As Figures 8a to 8c shown, when the vehicle formation is driving towards the target point and encounters a wall, it needs to turn and complete the turning process; as Figures 8d to 8f shown, the vehicle formation encounters the first obstacle and safely avoids the first obstacle; as Figures 8g to 8i shown, the vehicle formation encounters the second obstacle and safely avoids the second obstacle; as Figure 8j shown, after the formation avoids all obstacles, it reaches the target point and the formation task is completed.

[0135] During the above simulation process, the change of the path can be clearly seen in the Rviz 3D visualization platform. Figure 9 shows an optimal global path planned from the starting point to the target point based on the global A* algorithm. The part detected by the vehicle radar in the figure is also marked. As the vehicle moves in real time, the radar mark will also move in real time.

[0136] Figures 10a to 10d shows the offset of the optimal local path determined by the improved DWA algorithm adopted by the vehicle formation at different stages relative to the optimal global path. As Figure 10a shown, before the vehicle formation turns, it plans the turning path. Based on the improved DWA algorithm, on the premise of safely avoiding obstacles, the vehicle formation moves as little distance as possible. However, the vehicle formation as a whole has a width, so it only needs to ensure that the follower closest to the wall can move as little distance as possible while safely avoiding the wall to meet the requirements.

[0137] The vehicle formation plans the turning trajectory according to the above improved DWA algorithm and safely turns and successfully returns to the original trajectory according to the Figures 8a to 8c shown process.

[0138] As Figure 10bAs shown, the formation vehicle radar detects the first obstacle and starts planning a path to avoid the first obstacle. Based on the improved DWA algorithm, on the premise of safely avoiding the obstacle, the formation moves as little as possible. However, since the formation as a whole has a width, it only needs to ensure that the follower closest to the wall can move as little as possible while safely avoiding the obstacle to meet the requirements. The vehicle formation plans a trajectory to avoid the first obstacle according to the above improved DWA algorithm and returns to the original trajectory safely according to the Figures 8d to 8f process shown and successfully returns to the original trajectory.

[0139] As Figure 10c shown, the formation vehicle radar detects the second obstacle. Similar to avoiding the first obstacle, the formation plans a trajectory to avoid the second obstacle and returns to the original trajectory safely according to the Figures 8g to 8i process shown and successfully returns to the original trajectory. Finally, as Figure 10d shown, the formation reaches the target point and the formation mission is completed. This simulation can verify that the improved DWA algorithm does have certain effectiveness and practicability.

[0140] The specific implementation manners of the present application have been introduced in detail above. For those skilled in the art of this technology, without departing from the principle of the present application, several improvements and modifications can still be made to the present application, and these improvements and modifications also belong to the protection scope of the claims of the present application.

Claims

1. A vehicle formation driving control method based on an improved DWA algorithm, characterized in that It includes the following steps: S1. Establish a vehicle formation, where the vehicle formation includes at least one leading vehicle, and each leading vehicle includes at least one following vehicle; S2. The leading vehicle continuously travels according to the leading mode, and each following vehicle continuously travels according to the formation following mode until the vehicle formation reaches the target point and ends the travel, or any vehicle in the vehicle formation enters the mode switching range and enters step S3; S3. The vehicles that have not entered the mode switching range keep their driving modes unchanged, and the vehicles that enter the mode switching range continuously plan paths and travel according to the improved DWA algorithm until they drive out of the mode switching range and return to step S2; The vehicles that enter the mode switching range in step S3 continuously plan paths and travel according to the improved DWA algorithm until they drive out of the mode switching range and return to step S2, which specifically includes the following steps: S31. Sample the linear velocity v and angular velocity w of the vehicle that enters the mode switching range; S32. Determine each optional sampling speed combination based on the speed constraint conditions, and each sampling speed combination includes a group of optional linear velocity v and angular velocity w; S33. Simulate the driving trajectories corresponding to each optional sampling speed combination; S34. Determine the optimal driving trajectory based on the improved DWA evaluation function G(v, w) and control the vehicle in the mode switching range to travel along the optimal driving trajectory; S35. Judge whether the end point of the optimal driving trajectory has left the mode switching range. If so, execute step S2. If not, return to execute step S31; The specific form of the improved DWA evaluation function G(v, w) is: Among them, dist_goal(v, w) is the target point distance evaluation function, dist(v, w) is the obstacle distance evaluation function, velocity(v, w) is the speed evaluation function, ideal_way_goal(v, w) is the trajectory fitting degree evaluation function, α, β, γ, η are weight coefficients, and δ() is the normalization operation; The trajectory fitting degree evaluation function ideal_way_goal(v, w) is determined by the following formula: ideal_way_goal(v, w) = 1 / Max[Distance(Path1, Path2)], where Path1 is the driving trajectory obtained by simulating the sampling speed combination, Path2 is the driving trajectory determined based on the global planning, and Distance(Path1, Path2) is the distance between the corresponding nodes on the two trajectories.

2. The vehicle platoon driving control method based on the improved DWA algorithm according to claim 1, wherein When any vehicle in the vehicle formation enters the mode switching range, the distance between it and the obstacle is less than the mode switching threshold D shown in the following formula S : Among them, D min is the shortest distance for the arbitrary vehicle to plan a path to avoid obstacles, D max is the maximum distance for the arbitrary vehicle to maintain the formation in the vehicle formation, v max is the maximum linear speed of the arbitrary vehicle, v t is the real-time linear speed of the arbitrary vehicle, and a is an exponential constant.

3. The vehicle platoon driving control method based on the improved DWA algorithm according to claim 1, wherein The leading vehicle in step S2 continuously travels according to the leading mode, specifically: The leading vehicle continuously travels along the driving trajectory determined by the A* algorithm or the artificial potential field method.

4. The vehicle platoon driving control method based on the improved DWA algorithm according to claim 1, characterized in that, The following vehicles in step S2 continuously travel according to the formation following mode, specifically by cyclically executing the following steps: S21. Sample the positions, postures, linear velocities, and angular velocities of the following vehicle j and its leading vehicle i at the current moment respectively; S22. Determine the control quantity of the following vehicle j at the next moment based on the leader-following method, where the control quantity is the linear velocity and angular velocity of the vehicle; S23, respectively construct the relative distance potential field between the leading vehicle i and the following vehicle j and the relative angle potential field S24, respectively based on and determine the potential field force of the relative distance between the leading vehicle i and the following vehicle j and the potential field force of the relative angle obtain the total potential field force between the leading vehicle i and the following vehicle j S25, correct the control quantity of the following vehicle j by adjusting the total potential field force ​ S26. Control the following vehicle j based on the corrected control quantity.

Images