Dynamic Window Approach Based on Reduction of Collision Evaluation Window for Dynamic Obstacles

Abstract

The present invention discloses a dynamic window method based on the reduction of the dynamic obstacle collision assessment window, specifically a solution to the poor passability of an Ackermann intelligent vehicle when using the dynamic window method (DWA) for local path planning in the face of dynamic obstacles. For the Ackermann steering model, first, the minimum turning radius constraint condition is integrated into the speed-angular velocity constraint of the DWA algorithm; then, based on the velocity obstacle method, when the distance between the intelligent vehicle and the dynamic obstacle is less than the dangerous distance, the assessment of the dangerous state is carried out. If the intelligent vehicle is determined to be in a dangerous state, a future collision prediction assessment based on the simulated trajectory and the speed information of the intelligent vehicle and the obstacle is carried out. This process will eliminate the speed-angular velocity combinations with greater collision risks and narrow the speed-angular velocity window determined by the original DWA algorithm, enabling the intelligent vehicle to get out of the dangerous state fastest, so as to achieve the purpose of safely avoiding dynamic obstacles.

Description

Technical Field

[0001] The present invention relates to the technical fields of unmanned path planning and autonomous navigation, autonomous driving vehicles, and local obstacle avoidance path planning for mobile intelligent agents, and particularly relates to a dynamic window method based on reducing the collision assessment window for dynamic obstacles. In particular, it is a solution for poor passability when facing dynamic obstacles during local path planning of an Ackermann intelligent vehicle using the Dynamic Window Approach (DWA). Background Art

[0002] An intelligent vehicle is an integrated system that combines multiple functions such as environmental perception, dynamic decision-making and planning, behavior control and execution. It can make a target plan according to a pre-given task based on a known map, and continuously sense the information of the surrounding environment during driving to avoid obstacles, park, and track the road in real time and move forward along the correct path. Path planning is directly related to the driving trajectory and driving distance of the intelligent vehicle, and is of great significance to the research of intelligent vehicles. The so-called path planning is to avoid static obstacles through a certain algorithm in the environmental state with a given starting point and ending point, and find an optimal path. Path planning can be further divided into local path planning and global path planning.

[0003] The dynamic window method is a local path planning method and an in-built path planning algorithm for ROS robots.

[0004] In the Dynamic Window Approach (DWA), the evaluation function plays a decision-making role, including three parts: the orientation angle function, the obstacle function, and the speed function. The evaluation function relies entirely on the information at the current moment. When facing static obstacles, since the position of the obstacle itself does not change, the speed-angular velocity drive command calculated by the algorithm has good passability; while when facing dynamic obstacles, the DWA algorithm evaluation function only relies on the information at the current moment and lacks the defect of future state prediction, which will make its dynamic obstacle avoidance ability poor and result in low passability for dynamic obstacles. Summary of the Invention

[0005] The purpose of the present invention is to provide a collision prediction strategy based on integrating the motion state information of intelligent vehicles and dynamic obstacles, and a DWA algorithm that can effectively avoid dynamic obstacles for the defect that the existing DWA algorithm path planning algorithm lacks the ability to effectively avoid dynamic obstacles. That is, a dynamic window method based on reducing the collision assessment window for dynamic obstacles.

[0006] In view of the Ackermann steering model, the present invention first incorporates the minimum turning radius constraint condition into the speed-angular velocity constraint of the DWA algorithm; then, based on the velocity obstacle method, when the distance between the intelligent vehicle and the dynamic obstacle is less than the dangerous distance, a risk assessment is carried out. If the intelligent vehicle is determined to be in a dangerous state, a future collision prediction assessment based on the simulated trajectory and the speed information of the intelligent vehicle and the obstacle is carried out. This process will eliminate the speed-angular velocity combinations with relatively high collision risks, narrow the speed-angular velocity window determined by the original DWA algorithm, enable the intelligent vehicle to quickly get out of the dangerous state, so as to achieve the purpose of safely avoiding dynamic obstacles. Finally, through simulation on the Python3 platform, the effectiveness of the algorithm is verified.

[0007] To achieve the above object, the present invention adopts the following technical solutions:

[0008] A dynamic window method based on the reduction of the collision assessment window for dynamic obstacles, characterized in that:

[0009] Step S1: According to the minimum turning radius constraint condition of the Ackermann steering model, improve the speed-angular velocity constraint of the DWA algorithm; randomly initialize the map environment, initialize the speed v0, angular velocity w0, and orientation angle θ0 of the intelligent vehicle to 0, place the intelligent vehicle at the starting point P0, and set the end point to G;

[0010] Step S2: Considering the speed-angular velocity constraints of the limit speed-angular velocity, limit linear acceleration-angular acceleration, braking distance limit, and minimum turning radius limit, obtain the speed-angular velocity window at time t;

[0011] Step S3: Calculate the Euclidean distance |P1M1| between the intelligent vehicle and the dynamic obstacle at this time from the information of the position P0 of the intelligent vehicle and the position M0 of the dynamic obstacle; if |P1M1| is less than the safety distance D S and the current time is not the initial time 0, jump to step S6; otherwise, enter step S4;

[0012] Step S4: According to the speed-angular velocity window at time t obtained in step S2, or the improved speed-angular velocity window at time t obtained in step S7, perform sampling of the speed-angular velocity combination. Under the assumption of the motion model within a small interval Δ t calculate the simulated trajectory set τ at time t t ;

[0013] Step S5: Use the evaluation function to complete the scoring of the simulated trajectory set τ obtained in step S4 t select the group of speed-angular velocity combinations (v t , w t ) with the highest score, and drive the intelligent vehicle forward for a duration of Δ with (v t , w t ) t, the intelligent vehicle reaches position P t+1 ; If position P t+1 is not the end point G, then enter the (t + 1)th moment and repeat steps S1 - S5; if position P t+1 is the end point G, then terminate the loop;

[0014] Step S6: Conduct speed obstacle method analysis. If the intelligent vehicle is judged to be in a dangerous state, then enter Step S7; otherwise, enter Step S4;

[0015] Step S7: When the intelligent vehicle is in a dangerous state, conduct future collision prediction and evaluation based on the speed information of the simulated trajectory, the intelligent vehicle and the obstacle; this process reduces and improves the speed - angular velocity window at the tth moment in Step S2, and finally obtains the improved speed - angular velocity window at the tth moment and returns to Step S4.

[0016] Furthermore, the minimum turning radius constraint of the Ackermann steering model in Step S1 is specifically:

[0017] In the simplified single - vehicle model of the Ackermann intelligent vehicle, assume that the four wheels rotate around the same instantaneous center of rotation ICR. According to the geometric relationship, we have:

[0018]

[0019]

[0020] In the formula, L is the wheelbase, R is the turning radius, and α is the steering angle. When the turning radius R takes the minimum value r min , the steering angle α reaches the limit value α lim ;

[0021]

[0022] The absolute value of the limit angular velocity w lim that can be achieved under the known linear velocity v is:

[0023]

[0024] Therefore, the angular velocity constraint considering the minimum turning radius is expressed as the following formula:

[0025]

[0026] In summary, the speed - angular velocity window V v is reduced to:

[0027] V v = V m ∩V d ∩w a .

[0028] Further, the constraints of the limit speed-angular velocity, acceleration-limited speed-angular velocity, and braking distance-limited speed-angular velocity described in step S2 are specifically as follows:

[0029] (1) Limit speed-angular velocity

[0030] V m ={(v, w)|v ∈ [v min , v max ∧ w ∈ [w min , w max}

[0031] In the formula, [v min , v max is the limit linear velocity range, and [w min , w max is the limit angular velocity range; the speed-angular velocity that the intelligent vehicle can reach must be within its limit range;

[0032] (2) Acceleration-limited speed-angular velocity: The speed and angular velocity that the intelligent vehicle can reach within the next Δ t time are respectively constrained by the linear acceleration and angular acceleration:

[0033]

[0034] v c , w c are the current linear velocity and angular velocity, is the linear acceleration range, is the angular acceleration range;

[0035] (3) Braking distance-limited speed-angular velocity: Under the maximum deceleration, the intelligent vehicle should be able to decelerate to 0 before colliding with an obstacle:

[0036]

[0037] dist(v, w) is the closest distance from the trajectory corresponding to the speed-angular velocity combination (v, w) to the obstacle.

[0038] Further, the safety distance D S described in step S3 is specifically as follows:

[0039] D S = mΔ t V max

[0040] In the formula, m is the number of sampling steps of the simulated trajectory, Δ t is the sampling time interval of the simulated trajectory, and V max is the maximum linear velocity of the intelligent vehicle.

[0041] Further, the sampling of the speed-angular velocity combination in step S4 is specifically as follows:

[0042] In the speed-angular velocity window V at time t v t The speed range V referred to c , and the angular velocity range W c Under the condition, according to the speed resolution Δ v And the angular velocity Δ w The speed range V c And the angular velocity range W c Are discretized to form a speed set v c And an angular velocity set w c , and then the two sets are combined according to the one-to-one correspondence principle to form a speed-angular velocity combination (v t , w t ).

[0043] Further, the motion model assumption within the small interval Δ t In step S4 is specifically as follows:

[0044] When the motion time interval is small enough, the motion trajectory between two adjacent points is approximated as a uniform linear motion, and the specific formula is as follows:

[0045]

[0046] x t , y t Represent the horizontal and vertical coordinates of the intelligent vehicle at time t, Represents the heading angle at time t, v t , w t Represents the speed-angular velocity combination at time t.

[0047] Further, the evaluation function in step S5 is specifically as follows:

[0048] G(v, w) = σ(αheading(v, w) + ζdist(v, w) + γvel(v, w))

[0049] In the formula, σ() is a smoothing function, Represents the heading angle of the intelligent vehicle, δ is the angle between the line connecting the position of the intelligent vehicle and the target point and the positive x-axis direction, dist(v, w) is the Euclidean distance from the simulated trajectory to the nearest obstacle, vel(v, w) represents the magnitude of the linear velocity of the intelligent vehicle, and α, ζ, and γ are three weight coefficients.

[0050] Further, the speed obstacle method analysis in step S6 is specifically as follows:

[0051] Suppose the speed of the intelligent vehicle A at a certain moment is Its own size radius is r, and the position is (xo t , y o t ), the speed of the dynamic obstacle B is its own size radius is r1, and the position is (x b t , y b t ). Taking obstacle B as the reference system, the relative speed of intelligent vehicle A to obstacle B is After considering the volume of the intelligent vehicle, obstacle B expands its radius to r1 + r. Draw the tangents OT1 and OT2 of the circle of the expanded B through point A. If:

[0052]

[0053] It shows that if the motion states of the intelligent vehicle and the obstacle remain unchanged, a collision will occur, and it is determined that the intelligent vehicle is in a dangerous state at this moment.

[0054] Furthermore, the reduction and improvement of the future collision prediction evaluation and window in step S7 are specifically as follows:

[0055] Assume that the speed and angular velocity of the intelligent vehicle when entering the dangerous state are V0 and w0 respectively. According to the known conditions: the linear acceleration range is the angular acceleration range is the linear speed range is [V min , V max , the minimum turning radius r min , the limit turning angle is α lim , and the resolution of the speed and angular velocity of the dynamic window are Δ v 、Δ w , and obtain all the linear speed value sets V T of the linear speed window of the intelligent vehicle at this time as:

[0056]

[0057] In the Ackermann intelligent vehicle model, the angular velocity value set w T is affected by its speed magnitude, that is, the limit angular velocity should not exceed the angular velocity magnitude when driving at the limit linear speed and the minimum turning radius. Therefore, for any speed V r in the linear speed set, all the angular velocity value sets w T of the corresponding angular velocity window are:

[0058]

[0059] Take as the dangerous upper bound, as the dangerous lower bound, and take any speed The following analysis is carried out: Since the speed of the intelligent vehicle remains unchanged, it is necessary to change the current speed direction to get out of the dangerous state. There are two ways to get out of the dangerous state:

[0060] 1) Change the current speed to make the resultant speed overlap with the upper bound of danger and it is necessary to turn through an angle λ;

[0061] 2) Change the current speed to make the resultant speed overlap with the lower bound of danger and it is necessary to turn through an angle β;

[0062] Let the radius of circle M be Circle M intersects with the vector at C1 and E1. The resultant speed has two distribution cases: 1) λ < β, at this time, changing the direction to make overlap with is more likely to make the intelligent vehicle get out of the dangerous state; 2) λ ≥ β, at this time, changing the direction to make overlap with is more likely to make the intelligent vehicle get out of the dangerous state. In the dangerous state, if the current resultant speed is maintained, then a collision will occur after

[0063]

[0064] If it is necessary to ensure that the intelligent vehicle can get out of the dangerous state within time t d , it is necessary to turn counterclockwise through an angle λ or clockwise through an angle β:

[0065]

[0066]

[0067] The w r that satisfies the conditions is as shown in the following formula, where k is the angular velocity safety factor, and it is a constant between (0, 1):

[0068]

[0069] The same analysis is carried out for all the linear speeds in the linear speed value set V T , and finally the reduced angular velocity set w R is as follows:

[0070] w R ={w1, w2…w r}

[0071] r is the number of elements in the set of linear velocity values. The velocity-angular velocity window V v is updated to the following formula:

[0072] V v = V m ∩ V d ∩ w a ∩ w R .

[0073] Compared with the prior art, for the Ackermann steering model, the present invention and its preferred solutions first incorporate the minimum turning radius constraint condition into the velocity-angular velocity constraint of the DWA algorithm; then, based on the velocity obstacle method, when the distance between the intelligent vehicle and the dynamic obstacle is less than the dangerous distance, the assessment of the dangerous state is carried out. If the intelligent vehicle is determined to be in a dangerous state, the future collision prediction evaluation based on the simulation trajectory and the velocity information of the intelligent vehicle and the obstacle is carried out. This process will eliminate the velocity-angular velocity combinations with greater collision risks, narrow the velocity-angular velocity window determined by the original DWA algorithm, enable the intelligent vehicle to get out of the dangerous state fastest, and thus achieve the purpose of safely avoiding dynamic obstacles. Through the simulation on the Python3 platform, the effectiveness of the algorithm is verified. BRIEF DESCRIPTION OF THE DRAWINGS

[0074] Figure 1 is a schematic diagram of the simplified model of the Ackermann steering principle in the embodiment of the present invention;

[0075] Figure 2 is an analysis diagram of the velocity obstacle method in the embodiment of the present invention;

[0076] Figure 3 is an analysis diagram of dynamic collision assessment in the embodiment of the present invention;

[0077] Figure 4 is a schematic diagram of the simulation environment in the embodiment of the present invention;

[0078] Figure 5 is a schematic diagram of the simulation result of the original DWA algorithm;

[0079] Figure 6 is a schematic diagram of the simulation result of the improved DWA algorithm in the embodiment of the present invention;

[0080] Figure 7 is a schematic diagram of the implementation process in the embodiment of the present invention. DETAILED DESCRIPTION OF THE INVENTION

[0081] To make the features and advantages of this patent more obvious and understandable, the following specific embodiments are given for detailed description as follows:

[0082] It should be noted that the following detailed description is exemplary and is intended to provide further explanation of the present application. Unless otherwise specified, all technical and scientific terms used in this specification have the same meaning as commonly understood by those of ordinary skill in the technical field to which this application belongs.

[0083] It should be noted that the terms used herein are only for describing specific embodiments and are not intended to limit the exemplary embodiments according to the present application. As used herein, unless the context clearly indicates otherwise, the singular forms are also intended to include the plural forms. In addition, it should be understood that when the terms "comprising" and / or "including" are used in this specification, they indicate the presence of features, steps, operations, devices, components, and / or combinations thereof.

[0084] As Figure 7 shown, the solution provided by this embodiment specifically includes the following implementation steps:

[0085] Step S1: According to the minimum turning radius constraint condition of the Ackermann steering model, improve the speed-angular velocity constraint of the DWA algorithm. Randomly initialize the map environment, initialize the speed v0, angular velocity w0, and orientation angle θ0 of the intelligent vehicle to 0, place the intelligent vehicle at the starting point P0, and set the end point to G.

[0086] Step S2: Consider the speed-angular velocity constraints of the limit speed-angular velocity, limit linear acceleration-angular acceleration, braking distance limit, and minimum turning radius limit to obtain the speed-angular velocity window at time t.

[0087] The specific speed-angular velocity constraints of the limit speed-angular velocity and acceleration limit in Step S2 are as follows:

[0088] (1) Limit speed-angular velocity

[0089] V m ={(v, w)|v ∈ [v min , v max ∧ w ∈ [w min , w max}

[0090] In the formula, [v min , v max is the limit linear speed range, and [w min , w max is the limit angular velocity range. The specific meaning is that the speed-angular velocity that the intelligent vehicle can reach must be within its limit range.

[0091] (2) Acceleration-limited speed-angular velocity: The speed and angular velocity that the intelligent vehicle can reach in the next Δ t time are respectively constrained by the acceleration and angular acceleration:

[0092]

[0093] v c 、w c are the current linear velocity and angular velocity, is the linear acceleration range, is the angular acceleration range.

[0094] (3) Speed - angular velocity for braking distance limit: Under the maximum deceleration, the intelligent vehicle should be able to decelerate to 0 before colliding with an obstacle:

[0095]

[0096] dist(v, w) is the closest distance from the trajectory corresponding to the speed - angular velocity combination (v, w) to the obstacle.

[0097] In summary, the speed - angular velocity V at time t v window is expressed as:

[0098] V v = V m ∩V d ∩V a .

[0099] The specific minimum turning radius constraint of the Ackermann steering model described in step S1 is:

[0100] The simplified single - vehicle model of the Ackermann intelligent vehicle is as Figure 1 shown. The four wheels rotate around the same instantaneous center of rotation (ICR). According to geometric relationships, we can obtain

[0101]

[0102]

[0103] where L is the wheelbase, R is the turning radius, and α is the steering angle. When the turning radius R takes the minimum value r min , the steering angle α reaches the limit value α lim ;

[0104]

[0105] It can be known that the absolute value of the limit angular velocity w that can be achieved under the known linear velocity v lim is:

[0106]

[0107] Therefore, the angular velocity constraint considering the minimum turning radius is expressed as the following formula:

[0108]

[0109] In summary, the speed-angular velocity window V v is reduced to:

[0110] V v = V m ∩ V d ∩ w a .

[0111] Step S3: Calculate the Euclidean distance |P1M1| between the intelligent vehicle and the dynamic obstacle at this time from the information of the intelligent vehicle position P0 and the dynamic obstacle position M0. If |P1M1| is less than the safety distance D S and the current moment is not the initial moment 0, jump to Step S6; otherwise, enter Step S4.

[0112] The safety distance D mentioned in Step S3 S is specifically:

[0113] D S = mΔ t V max

[0114] where m is the number of sampling steps of the simulated trajectory, and Δ t is the sampling time interval of the simulated trajectory, and V max is the maximum linear speed of the intelligent vehicle.

[0115] Step S4: According to the speed-angular velocity window V v t at time t obtained in Step S2 (or the improved speed-angular velocity window at time t obtained in Step S7), perform sampling of the speed-angular velocity combination. Under the assumption of the motion model within a small interval Δ t , calculate the simulated trajectory set τ t .

[0116] The sampling of the speed-angular velocity combination mentioned in Step S4 is specifically:

[0117] Within the speed range V v t indicated by the speed-angular velocity window V c , and the angular velocity range W c , discretize the speed range V v and the angular velocity range W w according to the speed resolution Δ c and the angular velocity resolution Δ c to form a speed set v c and an angular velocity set w c , and then combine them in a one-to-one correspondence principle within these two sets to form a speed-angular velocity combination (v t , wt )。

[0118] The tiny interval Δ described in step S4 t The specific motion model assumption within is as follows:

[0119] When the motion time interval is small enough, the motion trajectory between adjacent two points can be approximated as a uniform linear motion. The specific formula is as follows:

[0120]

[0121] x t , y t represent the horizontal and vertical coordinates of the intelligent vehicle at time t, represents the heading angle at time t, v t , w t represent the speed-angular velocity combination at time t.

[0122] Step S5: Use the evaluation function to complete the scoring of the simulated trajectory set τ t obtained in step S4, select a set of speed-angular velocity combinations (v t , w t ) with the highest score, and drive the intelligent vehicle forward for a duration of Δ t , w t ). The intelligent vehicle reaches position P t . If position P t+1 is not the end point G, enter the (t + 1)th moment and repeat steps S1 - S5; if position P t+1 is the end point G, then terminate the loop. t+1

[0123] The specific evaluation function described in step S5 is as follows:

[0124] G(v, w) = σ(αheading(v, w) + ζdist(v, w) + γvel(v, w))

[0125] In the formula, σ() is a smoothing function, represents the heading angle of the intelligent vehicle, δ is the angle between the line connecting the position of the intelligent vehicle and the target point and the positive x-axis direction, dist(v, w) is the Euclidean distance from the simulated trajectory to the nearest obstacle, vel(v, w) represents the magnitude of the linear velocity of the intelligent vehicle, and α, ζ, γ are three weight coefficients.

[0126] Step S6: Conduct speed obstacle method analysis. If the intelligent vehicle is determined to be in a dangerous state, enter step S7; otherwise, enter step S4.

[0127] The specific speed obstacle method analysis described in step S6 is as follows:

[0128] Such as Figure 2 ​As shown in the figure, let the speed of the intelligent vehicle A at this moment be its own size radius is r, and the position is (x o t , y o t ), and the speed of the dynamic obstacle B is its own size radius is r1, and the position is (x b t , y b t ). Taking the obstacle B as the reference system, the relative speed of the intelligent vehicle A to the obstacle B is After considering the volume of the intelligent vehicle, the obstacle B expands the radius to r1 + r. Draw the tangents OT1 and OT2 of the expanded circle of B passing through point A. If

[0129]

[0130] It shows that if the motion states of the intelligent vehicle and the obstacle remain unchanged, a collision will occur, and it is determined that the intelligent vehicle is in a dangerous state at this moment.

[0131] Step S7: When the intelligent vehicle is in a dangerous state, conduct a future collision prediction and evaluation based on the simulated trajectory and the speed information of the intelligent vehicle and the obstacle. This process will reduce and improve the speed-angular velocity window at time t described in step S2, and finally obtain the improved speed-angular velocity window at time t, and return to step S4.

[0132] The specific reduction and improvement of the future collision prediction evaluation and window in step S7 are as follows:

[0133] Assume that the speed and angular velocity of the intelligent vehicle when entering the dangerous state are V0 and w0 respectively. According to the known conditions: the linear acceleration range is the angular acceleration range is the linear velocity range is [V min , V max , the minimum turning radius r min , the limit turning angle is α lim , and the resolutions of the speed and angular velocity of the dynamic window are Δ v and Δ w respectively. It can be obtained that the set V T of all linear velocity values of the linear velocity window of the intelligent vehicle at this time is:

[0134]

[0135] Since the research object is an Ackermann intelligent vehicle, the set w T of its angular velocity values is affected by the magnitude of its speed, that is, the limit angular velocity should not exceed the angular velocity magnitude when traveling at the limit linear velocity and the minimum turning radius. Therefore, for any speed V rThe set \(w\) of all angular velocity values corresponding to the angular velocity window T is as follows:

[0136]

[0137] As Figure 3 shown, is regarded as the upper dangerous bound, is regarded as the lower dangerous bound, and any velocity in the linear velocity set is taken for the following analysis: Since the speed of the intelligent vehicle does not change in magnitude, it is necessary to change the current speed direction to get out of the dangerous state. There are two ways to get out of the dangerous state:

[0138] 3) Change the current speed so that the resultant speed overlaps with the upper dangerous bound and the angle \(\lambda\) needs to be turned;

[0139] 4) Change the current speed so that the resultant speed overlaps with the lower dangerous bound and the angle \(\beta\) needs to be turned;

[0140] Let the radius of the circle \(M\) be The circle \(M\) intersects the vector at \(C1\) and \(E1\). The resultant speed has two distribution cases: 1) \(\lambda\lt\beta\). At this time, changing the direction to make overlap with is more likely to make the intelligent vehicle get out of the dangerous state; 2) \(\lambda\geq\beta\). At this time, changing the direction to make overlap with is more likely to make the intelligent vehicle get out of the dangerous state. In the dangerous state, if the current resultant speed is maintained, then a collision will occur after

[0141]

[0142] If the intelligent vehicle is to get out of the dangerous state within the time \(t\) d , it is necessary to turn counterclockwise by an angle \(\lambda\) or clockwise by an angle \(\beta\):

[0143]

[0144]

[0145] The \(w\) that satisfies the condition r is shown in the following formula, where \(k\) is the angular velocity safety factor, and a constant between \((0,1)\) is taken:

[0146]

[0147] For all the linear velocities in the set V of linear velocity values T perform the same analysis, and finally obtain the reduced angular velocity set w R as follows:

[0148] w R = {w1, w2 … w r}

[0149] r is the number of elements in the set of linear velocity values. The velocity-angular velocity window V v is updated to the following formula:

[0150] V v = V m ∩ V d ∩ w a ∩ w R .

[0151] To verify the effectiveness of the algorithm proposed in the present invention, based on the python3 language platform, a complex map environment containing dynamic obstacles is randomly initialized, specifically as Figure 1 shown. The starting point of the intelligent vehicle is set to (-25, 25), the ending point is set to (30, 30), the black circles are randomly initialized obstacles with a radius of 0.5 m for themselves, the green circles are dynamic obstacles, as shown by A, B, and C in the figure, and their movement trajectories and directions are as shown in the figure. After that, the comparison of the experiments between the algorithm proposed in the present invention and the original DWA algorithm is as follows

[0152] Table 2 Simulation experiment table

[0153]

[0154] The comparative experiment was carried out 6000 times in total. The arrival rate of the improved DWA algorithm increased by 108.83%, while the path efficiency and time efficiency decreased by 1.87% and 1.31% respectively. For the comparison of the latter two items, only the data of reaching the ending point are counted. Due to the random generation of the map, there is a deviation in map complexity in the comparison of path length and time efficiency. The arrival rate of the original DWA algorithm is low, and a large amount of data on average path length and average time comes from simple maps. Therefore, the increase in the arrival rate has more significant significance. Figure 5 is the simulation result of the circular DWA algorithm, colliding with obstacle B; Figure 6 is the simulation result of the improved DWA algorithm, successfully avoiding three dynamic obstacles and reaching the ending point.

[0155] The above are only the preferred embodiments of the present invention, and are not intended to limit the present invention in other forms. Any person skilled in the art may use the disclosed technical content to make changes or modifications into equivalent embodiments with equivalent changes. However, any simple modifications, equivalent changes and modifications made to the above embodiments based on the technical essence of the present invention without departing from the technical solution content of the present invention still fall within the protection scope of the technical solution of the present invention.

[0156] This patent is not limited to the above best implementation manner. Anyone inspired by this patent can obtain various other forms of the dynamic window method based on the reduction of the dynamic obstacle collision assessment window. All equal changes and modifications made according to the scope of the patent application of the present invention shall fall within the scope covered by this patent.

Claims

1. A dynamic window method based on the reduction of the dynamic obstacle collision assessment window, characterized in that: Step S1: According to the minimum turning radius constraint condition of the Ackermann steering model, improve the speed-angular velocity constraint of the DWA algorithm; randomly initialize the map environment, initialize the speed v0, angular velocity w0, and heading angle θ0 of the intelligent vehicle to 0, place the intelligent vehicle at the starting point P0, and set the end point to G; Step S2: Obtain the speed-angular velocity window at time t considering the speed-angular velocity constraints of the limit speed-angular velocity, limit linear acceleration-angular acceleration, braking distance limit, and minimum turning radius limit; Step S3: Calculate the Euclidean distance |P1M1| between the intelligent vehicle and the dynamic obstacle at this time from the information of the position P0 of the intelligent vehicle and the position M0 of the dynamic obstacle; if |P1M1| is less than the safety distance D S and the current moment is not the initial moment 0, jump to step S6; otherwise, enter step S4; Step S4: According to the velocity-angular velocity window at time t obtained in step S2, or the improved velocity-angular velocity window at time t obtained in step S7, sample the velocity-angular velocity combination, and under the assumption of the motion model within a tiny interval Δ t , calculate the simulated trajectory set τ t at time t; Step S5: Use the evaluation function to complete the scoring of the simulated trajectory set τ t obtained in step S4, and select the set of velocity-angular velocity combinations (v t , w t ) with the highest score, and drive the intelligent vehicle forward for a duration of Δ t with (v t , w t ). The intelligent vehicle reaches the position P t+1 ; if the position P t+1 is not the end point G, then enter the t+1 moment and repeat steps S1-S5; if the position P t+1 is the end point G, then terminate the loop; Step S6: Conduct speed obstacle method analysis. If the intelligent vehicle is determined to be in a dangerous state, enter Step S7; Otherwise, enter Step S4; Step S7: When the intelligent vehicle is in a dangerous state, conduct future collision prediction and evaluation based on the simulation trajectory and the speed information of the intelligent vehicle and the obstacle; this process reduces and improves the speed-angular velocity window at time t in Step S2, and finally obtains the improved speed-angular velocity window at time t, and returns to Step S4.

2. The dynamic window method based on the reduction of the dynamic obstacle collision assessment window according to claim 1, characterized in that: The minimum turning radius constraint of the Ackermann steering model in Step S1 is specifically: In the simplified single-vehicle model of the Ackermann intelligent vehicle, assuming that the four wheels rotate around the same instantaneous rotation center ICR, according to the geometric relationship: where L is the wheelbase, R is the turning radius, and α is the steering angle. When the turning radius R reaches the minimum value r min , the steering angle α reaches the limit value α lim ; The absolute value of the limiting angular velocity ω that can be achieved under the condition of a known linear velocity v lim is as follows: Therefore, the angular velocity constraint considering the minimum turning radius is expressed as the following formula: In summary, the speed-angular velocity window V v is reduced to: V v = V m ∩V d ∩w a 。 3. The dynamic window method based on the reduction of the dynamic obstacle collision assessment window according to claim 1, characterized in that: The speed-angular velocity constraints of the limit speed-angular velocity, acceleration limit, and braking distance limit in Step S2 are specifically: (1) Limit speed-angular velocity V m = {(v, w) | v ∈ [v min , v max ∧ w ∈ [w min , w max} where [v min , v max is the limit linear velocity range, and [w min , w max is the limit angular velocity range; the speed-angular velocity that the intelligent vehicle can reach must be within its limit range; (2) Acceleration-limited speed-angular velocity: The speed and angular velocity that the intelligent vehicle can reach within the next Δ t time are respectively constrained by the acceleration and angular acceleration: v c 、w c are the current linear velocity and angular velocity, is the linear acceleration range, is the angular acceleration range; (3) Speed-angular velocity constraint of braking distance limit: Under the maximum deceleration, the intelligent vehicle should be able to decelerate to 0 before colliding with the obstacle: dist(v, w) is the closest distance between the trajectory corresponding to the speed-angular velocity combination (v, w) and the obstacle.

4. The dynamic window method based on the reduction of the dynamic obstacle collision assessment window according to claim 1, characterized in that: The safety distance D described in step S3 S Specifically: D S = mΔ t V max where m is the number of sampling steps of the simulated trajectory, and Δ t is the sampling time interval of the simulated trajectory, and V max is the maximum linear velocity of the intelligent vehicle.

5. The dynamic window method based on the reduction of the dynamic obstacle collision assessment window according to claim 1, characterized in that: The sampling of the speed-angular velocity combination in Step S4 is specifically: Velocity - angular velocity window V at time t v t The indicated velocity range V c , angular velocity range W c , according to the velocity resolution Δ v and angular velocity Δ w Discretize the velocity range V c and the angular velocity range W c to form a velocity set v c and an angular velocity set w c . Then, within these two sets, combine them according to the one - to - one correspondence principle to form velocity - angular velocity combinations (v t , w t ).

6. The dynamic window method based on the reduction of the dynamic obstacle collision assessment window according to claim 1, characterized in that: The motion model assumption within the minute interval Δ described in step S4 t is specifically as follows: When the movement time interval is small enough, the movement trajectory between two adjacent points is approximated as a uniform linear motion, and the specific formula is as follows: x t 、y t represent the horizontal and vertical coordinates of the intelligent vehicle at time t, represent the heading angle at time t, v t 、w t represent the speed-angular velocity combination at time t.

7. The dynamic window method based on the reduction of the dynamic obstacle collision assessment window according to claim 1, characterized in that: The evaluation function in Step S5 is specifically: G(v, w) = σ(αheading(v, w) + ζdist(v, w) + γvel(v, w)) where σ() is a smoothing function, represents the heading angle of the intelligent vehicle, δ is the angle between the line connecting the position of the intelligent vehicle and the target point and the positive x-axis direction, dist(v, w) is the Euclidean distance from the simulated trajectory to the nearest obstacle, vel(v, w) represents the magnitude of the linear velocity of the intelligent vehicle, and α, ζ, and γ are three weight coefficients.

8. The dynamic window method based on the reduction of the dynamic obstacle collision assessment window according to claim 1, characterized in that: The speed obstacle method analysis in Step S6 is specifically: Suppose the speed of intelligent vehicle A at a certain moment is Its own size radius is r, and its position is (x o t , y o t ), the speed of dynamic obstacle B is Its own size radius is r1, and its position is (x b t , y b t ). Taking obstacle B as the reference frame, the relative speed of intelligent vehicle A to obstacle B is After considering the volume of the intelligent vehicle, obstacle B expands its radius to r1 + r. Draw the tangents OT1 and OT2 of the expanded circle of B passing through point A. If: It is explained that if the motion states of the intelligent vehicle and the obstacle remain unchanged, a collision will occur, and it is determined that the intelligent vehicle is in a dangerous state at this moment.

9. The dynamic window method based on the reduction of the dynamic obstacle collision assessment window according to claim 8, characterized in that: The specific improvement of the future collision prediction assessment and window reduction in step S7 is as follows: Let the speed and angular velocity of the intelligent vehicle when entering the dangerous state be \(V_0\) and \(\omega_0\) respectively. According to the known conditions: the linear acceleration range is the angular acceleration range is the linear velocity range is \([V min , V max , the minimum turning radius \(r min , the limit turning angle is \(\alpha lim , and the resolutions of the speed and angular velocity of the dynamic window are \(\Delta v \), \(\Delta w respectively. Obtain all the linear velocity value sets \(V T of the intelligent vehicle's linear velocity window at this time as: In the Ackermann intelligent vehicle model, the angular velocity value set w T is affected by its speed magnitude, that is, the limit angular velocity should not exceed the angular velocity magnitude when driving at the limit linear velocity and the minimum turning radius. Therefore, for any velocity V in the linear velocity set r the set of all angular velocity values w of the corresponding angular velocity window T is as follows: Regard as the upper bound of danger, regard it as the lower bound of danger, and take any speed in the linear velocity set for the following analysis: Since the speed of the intelligent vehicle remains unchanged in magnitude, it is necessary to change the current speed direction to get out of the dangerous state. There are two ways to get out of the dangerous state: 1) Change the current speed Make the resultant speed Overlap with the upper bound of danger and a rotation angle λ is required; 2) Change the current speed Make the combined speed Overlap with the lower bound of danger and it is necessary to turn through an angle β; Let the radius of circle M be Circle M intersects with vector at C1 and E1, and the resultant velocity has two distribution cases: 1) λ < β, in which case changing the direction so that overlaps with more easily enables the intelligent vehicle to get out of the dangerous state; 2) λ ≥ β, in which case changing the direction so that overlaps with more easily enables the intelligent vehicle to get out of the dangerous state. In the dangerous state, if the current resultant velocity is maintained, then in A collision will occur later. If we want to ensure that the intelligent vehicle can get out of the dangerous state within time t, it needs to turn counterclockwise by an angle λ or clockwise by an angle β: d ​ w that meets the conditions r As shown in the following formula, where k is the angular velocity safety factor, which is a constant between (0, 1): For all the linear velocities in the set V of linear velocity values T perform the same analysis, and finally obtain the reduced set w of angular velocities R as follows: w R = {w1, w2… w r} r is the number of elements in the set of linear velocity values. The velocity-angular velocity window V v is updated to the following formula: V v = V m ∩V d ∩w a ∩w R 。

Images