A collision avoidance method in dynamic environment

Abstract

The present invention discloses a collision avoidance method for dynamic environments, including: a collision avoidance rule applicable to all encounter scenarios, regardless of whether obstacles comply with the collision avoidance rule; when an autonomous device encounters an obstacle while tracking a desired trajectory, a collision risk assessment is performed to mark obstacles with a collision risk as active. The proposed collision avoidance method calculates an optimal collision avoidance speed that avoids all activated obstacles based on the autonomous device's desired speed and the collision avoidance rule.

Description

Technical Field

[0001] The invention relates to a collision avoidance method in a dynamic environment. Background Art

[0002] Velocity Obstacle (VO): This method generates a cone-shaped obstacle in velocity space (hence the name velocity barrier) that guarantees that no future collisions will occur as long as the autonomous vehicle’s velocity vector is outside the VO.

[0003] from Start to The ray direction is expressed as:

[0004] Given an autonomous device of shape A with a speed Move, given an obstacle of shape B with speed Moving, the safe distance between the autonomous equipment boundary and the obstacle boundary is d and Then the danger zone of B is

[0005] Minkowski and:

[0006] Mirror Image:

[0007] Obstacle expansion: When A is simplified to a point, the danger zone of B relative to A is

[0008] Therefore, VO of B in the velocity space of A is

[0009] B's VO in the relative velocity space of A relative to B is like Figure 1 Shown is a schematic diagram of obstacle expansion and speed barrier.

[0010] Due to their limitations, most current collision avoidance methods can only be applied to relatively simple scenarios, such as avoiding static obstacles or single dynamic obstacles. They cannot provide reliable collision avoidance performance when multiple dynamic obstacles need to be avoided. Furthermore, some collision avoidance methods do not consider collision avoidance rules or the rules they consider are too complex and cumbersome, making them unsuitable for practical engineering applications. Summary of the Invention

[0011] To solve the problems existing in the above-mentioned prior art, the present invention proposes a collision avoidance method in a dynamic environment. The purpose is to apply the proposed collision avoidance method to re-plan the optimal collision avoidance speed that complies with the collision avoidance rules based on the expected speed when the autonomous equipment encounters single or multiple dynamic or static obstacles while tracking the expected trajectory to avoid the obstacles.

[0012] The present invention can be implemented through the following technical solutions:

[0013] A collision avoidance method in a dynamic environment comprises the following steps:

[0014] 1) Project the obstacles detected by the autonomous equipment into the velocity space and expand them to form VO O ;

[0015] 2) determining, through collision risk assessment, whether an obstacle detected by the autonomous device poses a risk of collision with the autonomous device; if the detected obstacle poses a risk of collision with the autonomous device, marking its status as active; otherwise, marking its status as inactive;

[0016] 3) updating the state of the autonomous device, whereby the autonomous device is in a collision avoidance state as long as there is an activated obstacle, otherwise it is in a non-collision avoidance state and tracks the desired trajectory;

[0017] 4) when the autonomous device is in a collision avoidance state, determining an optimal offset speed direction based on its desired speed and all activated obstacle information;

[0018] 5) For each activated obstacle, calculate its cone VO according to the desired speed of the autonomous device and the optimal bias speed direction O Two collision avoidance speeds corresponding to both sides;

[0019] 6) Determine collision avoidance rules;

[0020] 7) establishing a cost function using the proposed collision avoidance rule and finding the optimal collision avoidance speed from the previously calculated collision avoidance speeds;

[0021] 8) The autonomous equipment tracks the optimal collision avoidance speed until its state is updated to non-collision avoidance.

[0022] Furthermore, the step 4) is specifically as follows: Defined as a unit vector parallel to the bias velocity, the desired velocity of the autonomous device and the obstacle velocity are and The desired relative speed of the autonomous device relative to the obstacle In order to make the replanned trajectory of the autonomous device closer to the desired trajectory while avoiding obstacles, the bias speed should be as small as possible, so and The angle between the straight lines should be as large as possible. When the autonomous device encounters a single activated obstacle, set When the autonomous device encounters multiple activated obstacles, the relative speed between the autonomous device and each activated obstacle is determined. direction.

[0023] Furthermore, the The specific method for determining the direction of is as follows: Each element is the relative speed between the autonomous device and each activated obstacle, n is the number of activated obstacles, and a 2×n matrix A=[A1 A2] is defined. T ,in,

[0024]

[0025] Then, the matrix A is arranged from small to large according to the first row to obtain the matrix B. When the relative speed of the autonomous equipment and each activated obstacle is parallel, Same as when the autonomous device encounters a single activated obstacle, that is, when B(1, 1) = B(1, n), Otherwise, define a 2×n matrix C = [C1 ... C n-1 C n ],in And arrange the matrix C from large to small according to the second row to get the matrix D. At this time,

[0026] Furthermore, the step 5) is specifically as follows: considering that the maximum translation speed of the autonomous equipment is U max , and Respectively In addition The two limit speeds that can be achieved after the offset speed in the direction, and For activated obstacles, and Respectively In addition The left and right limit speeds that can be achieved after the offset speed in the direction; Indicates that the autonomous equipment is from VO O The offset speed when avoiding collision on the left side, and is the corresponding relative speed, Indicates independent equipment from VO O The offset speed when avoiding collision on the right side, and is the corresponding relative speed,

[0027] and It is obtained from the following conditions:

[0028]

[0029] Furthermore, considering the maximum translation speed U of the autonomous equipment max restrictions, and are constrained as follows:

[0030]

[0031]

[0032] Therefore, you can get independent equipment from VO O Collision avoidance speed when performing collision avoidance on the left and right sides and

[0033] Furthermore, the collision avoidance rule is: when an obstacle needs to be avoided, no matter from which direction the obstacle approaches, and no matter whether the obstacle takes collision avoidance measures, the autonomous equipment shall give priority to the VO of the obstacle. O Avoid on the right side.

[0034] Furthermore, the step 7) is specifically as follows:

[0035] definition Each element represents the speed of each activated obstacle, defined as Each element represents the VO of the autonomous device from each obstacle. O The collision avoidance speed when avoiding collision on the left or right side is calculated by establishing the cost function J=J1+J2+J3 from V c0 Select the optimal collision avoidance speed.

[0036] Furthermore, in the cost function:

[0037] Define a 1×2n matrix J1 with weights w1, where Define the 1×2n matrix J2 by ​​weight w2, where If the collision avoidance speed V c0 (i) It will put the autonomous device in the VO of other obstacles o The collision point where the collision is about to occur is (V c0 (i)-V o (i)) with the VO o The nearest intersection point of represents the position vector from the current position of the autonomous device to the collision point, and the collision avoidance reaction time is Otherwise c =w3, giving priority to a longer collision avoidance reaction time, and defining a 1×2n matrix J3 by weight w3, where J3(i)=w3-t c ; Therefore, define the 2×2n matrix E=[V c0 J] T, and the matrix E is arranged from small to large according to the second row to obtain the matrix F, the optimal collision avoidance speed is F(1, 1), and the autonomous equipment tracks the optimal collision avoidance speed until its state is updated to non-collision avoidance, thereby achieving the collision avoidance purpose.

[0038] Beneficial effects

[0039] 1) The collision avoidance rules proposed in the present invention are suitable for all encounter situations and are simpler than traditional ones.

[0040] 2) The collision avoidance method proposed in the present invention can provide stable and reliable collision avoidance performance in single or multiple dynamic or static obstacle environments, has a wide range of applications, and is simple and easy to implement. BRIEF DESCRIPTION OF THE DRAWINGS

[0041] Figure 1 Schematic diagram of obstacle expansion and speed barrier in background technology;

[0042] Figure 2-1 A schematic diagram of the autonomous device in the present invention selecting to go around one of the obstacles;

[0043] Figure 2-2 Schematic diagram of the collision avoidance speed corresponding to each activated obstacle in the present invention;

[0044] Figure 3 This is a flow chart of the collision avoidance method of the present invention;

[0045] Figure 4-1 A schematic diagram of determining the optimal biased velocity direction when the autonomous device encounters a single activated obstacle in the present invention;

[0046] Figure 4-2 This is the first case when the autonomous equipment encounters multiple activated obstacles in the present invention. Schematic diagram;

[0047] Figure 4-3 This is the second case when the autonomous equipment encounters multiple activated obstacles in the present invention. Schematic diagram;

[0048] Figure 5-1 is the maximum translation speed based on autonomous equipment in the present invention, and In addition Schematic diagram of the maximum speed that can be achieved after the offset speed in the direction;

[0049] Figure 5-2 Schematic diagram of the collision avoidance speed for avoiding a single activated obstacle calculated based on the desired speed and optimal offset speed direction of the autonomous equipment in the present invention;

[0050] Figure 6The collision avoidance rules of the present invention include: (a) the obstacle passes from the right. (b) the obstacle passes from the left. (c) the obstacle is overtaken. (d) the obstacle approaches head-on. (e) the obstacle is overtaken. DETAILED DESCRIPTION

[0051] The following describes the embodiments of the present invention through specific embodiments. Those skilled in the art can easily understand other advantages and effects of the present invention from the contents disclosed in this specification.

[0052] The present invention proposes a new collision avoidance method for autonomous equipment in single or multiple dynamic or static obstacle environments. Different from the existing collision avoidance methods, this method is based on the premise that the autonomous equipment tracks the desired trajectory. When collision avoidance is required, the optimal collision avoidance speed is obtained by adding a bias speed to the desired speed of the autonomous equipment, and the collision avoidance purpose can be achieved by making it track the optimal collision avoidance speed. Obstacles that have a collision risk with the autonomous equipment are marked as activated through collision risk assessment. The direction of the bias speed is calculated from the information of all activated obstacles. When the autonomous equipment encounters multiple obstacles, it will only choose to go around one of the obstacles without colliding with any obstacle, such as Figure 2-1 Therefore, the collision avoidance speed that can avoid a single activated obstacle can be calculated separately, as shown in Figure 2-2 Then, a cost function is established based on the proposed collision avoidance rule and the optimal collision avoidance speed is selected from it.

[0053] Represents a vector The symbol '\' represents the remainder. Represents a vector The flowchart of the collision avoidance method in a dynamic environment of the present invention is as follows: Figure 3 The specific steps are as follows:

[0054] 1) In order to facilitate the calculation of the collision avoidance speed and enable the autonomous equipment to maintain a safe distance from obstacles while tracking the optimal collision avoidance speed, the detected obstacles are projected into the velocity space and expanded to form VO o ;

[0055] 2) Determine whether the obstacle detected by the autonomous device poses a risk of collision with it through collision risk assessment. If the detected obstacle poses a risk of collision with the autonomous device, mark its status as active; otherwise, mark its status as inactive.

[0056] 3) Update the state of the autonomous device. As long as there is an activated obstacle, the autonomous device is in the collision avoidance state. Otherwise, it is in the non-collision avoidance state and tracks the desired trajectory.

[0057] 4) When the autonomous device is in a collision avoidance state, the optimal offset speed direction is determined based on its desired speed and all activated obstacle information:

[0058] Will is defined as a unit vector parallel to the bias velocity. The desired velocity of the autonomous vehicle and the obstacle velocity are and The desired relative speed of the autonomous vehicle relative to the obstacle

[0059] In order to make the replanned trajectory of the autonomous equipment closer to the desired trajectory while avoiding obstacles, the bias speed should be as small as possible, so and The angle of the straight line should be as large as possible. When the autonomous device encounters a single activated obstacle, we can set like Figure 4-1 When the autonomous device encounters multiple activated obstacles, it is necessary to determine the relative speed between the autonomous device and each activated obstacle. direction. The method for determining is as follows: Define Each element is the relative speed between the autonomous device and each activated obstacle, and n is the number of activated obstacles. And define a 2×n matrix A = [A1 A2] T ,in,

[0060]

[0061] Then, the matrix A is arranged from small to large according to the first row to obtain the matrix B. When the relative speed of the autonomous equipment and each activated obstacle is parallel, Same as when the autonomous device encounters a single activated obstacle. That is, when B(1, 1) = B(1, n), Otherwise, define a 2×n matrix C=[C1... C n-1 C n ],in And arrange the matrix C from large to small according to the second row to get the matrix D. At this time, Figure 4-2 , 4-3 respectively show the two cases

[0062] 5) For each activated obstacle, calculate the cone VO according to the desired speed of the autonomous device and the optimal bias speed direction O The two collision avoidance speeds corresponding to the two sides:

[0063] Considering the maximum translation speed of the autonomous equipment is Umax , and Respectively In addition The two limit speeds that can be achieved after the offset speed in the direction, and For activated obstacles, and Respectively In addition The left and right limit speeds that can be achieved after the offset speed in the direction are as follows: Figure 5-1 shown. Indicates independent equipment from VO O The offset speed when avoiding collision on the left side, and is the corresponding relative speed. Indicates independent equipment from VO O The offset speed when avoiding collision on the right side, and is the corresponding relative speed, such as Figure 5-2 shown.

[0064] and It is obtained from the following conditions:

[0065]

[0066] Furthermore, considering the maximum translation speed U of the autonomous equipment max restrictions, and are constrained as follows:

[0067]

[0068]

[0069] Therefore, you can get independent equipment from VO o Collision avoidance speed when performing collision avoidance on the left and right sides and

[0070] 6) Determine collision avoidance rules

[0071] In actual encounters, obstacles may be static or dynamic, and dynamic obstacles may or may not take collision avoidance measures for the autonomous device. In order to make the collision avoidance measures of the autonomous device adaptable to all encounter situations, the following collision avoidance rules are assumed: when it is necessary to avoid an obstacle, no matter from which direction the obstacle approaches and whether the obstacle takes collision avoidance measures, the autonomous device will give priority to the VO of the obstacle. O Avoid to the right of Figure 6As shown in the figure (1 is the autonomous device, 2 is the obstacle, where: (a) the obstacle is crossed from the right; (b) the obstacle is crossed from the left; (c) the obstacle is overtaken; (d) the obstacle is approached head-on; (e) the obstacle is overtaken). This collision avoidance rule has two main advantages: 1. It does not require judgment of the encounter situation, and the rules are simple, which facilitates the implementation of collision avoidance methods. 2. If both parties in the encounter comply with the collision avoidance rule, they will avoid each other, thereby reducing the avoidance pressure on the avoiding party.

[0072] 7) Establish a cost function through the proposed collision avoidance rule and find the optimal collision avoidance speed from the previously calculated collision avoidance speeds: Define Each element represents the velocity of each activated obstacle. Each element represents the VO of each obstacle from the autonomous equipment. O The collision avoidance speed when avoiding collision on the left or right side. By establishing the cost function J = J1 + J2 + J3, from V c0 The optimal collision avoidance speed is selected from the equations . The cost function considers the following three aspects:

[0073] 1) Bias speed

[0074] In order to reduce the collision avoidance pressure of the autonomous equipment, a smaller bias speed is preferred. The 1×2n matrix J1 is defined by the weight w1, where

[0075] 2) Collision avoidance rules

[0076] When the autonomous device needs to avoid a dynamic obstacle, it will prioritize the VO of the obstacle. o The right side is used for collision avoidance. Define the 1×2n matrix J2 by ​​weight w2, where

[0077] 3) Reaction time to collision with other obstacles

[0078] If the collision avoidance speed V c0 (i) It will put the autonomous equipment in the VO of other obstacles O The collision point where the collision is about to occur is (V c0 (i)-V o (i)) with the VO O The nearest intersection point of represents the position vector from the current position of the autonomous equipment to the collision point, and the collision avoidance reaction time is Otherwise c =w3. A longer collision avoidance reaction time is preferred. Define a 1×2n matrix J3 by weight w3, where J3(i)=w3-t c .

[0079] Therefore, define the 2×2n matrix E=[V c0 J] T , and arrange the matrix E from small to large according to the second row to obtain the matrix F. The optimal collision avoidance speed is F(1,1).

[0080] 8) The autonomous device tracks the optimal collision avoidance speed until its status is updated to non-collision avoidance.

[0081] The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. Any modifications, equivalent substitutions and improvements made within the spirit and principles of the present invention should be included in the scope of protection of the present invention.

Claims

1. A collision avoidance method in a dynamic environment, characterized in that: The following steps are involved: 1) Project the obstacles detected by the autonomous equipment into the velocity space and expand them to form VO O ; 2) determining, through collision risk assessment, whether an obstacle detected by the autonomous device poses a risk of collision with the autonomous device; if the detected obstacle poses a risk of collision with the autonomous device, marking its status as active; otherwise, marking its status as inactive; 3) updating the state of the autonomous device, whereby the autonomous device is in a collision avoidance state as long as there is an activated obstacle, otherwise it is in a non-collision avoidance state and tracks the desired trajectory; 4) when the autonomous device is in a collision avoidance state, determining an optimal offset speed direction based on its desired speed and all activated obstacle information; 5) For each activated obstacle, calculate its cone VO according to the desired speed of the autonomous device and the optimal bias speed direction O Two collision avoidance speeds corresponding to both sides; 6) Determine collision avoidance rules; 7) establishing a cost function using the proposed collision avoidance rule and finding the optimal collision avoidance speed from the previously calculated collision avoidance speeds; 8) The autonomous device tracks the optimal collision avoidance speed until its state is updated to non-collision avoidance; The step 4) is specifically as follows: Will Defined as a unit vector parallel to the bias velocity, the desired velocity of the autonomous device and the obstacle velocity are and The desired relative speed of the autonomous device relative to the obstacle In order to make the replanned trajectory of the autonomous device closer to the desired trajectory while avoiding obstacles, the bias speed should be as small as possible, so and The angle between the straight lines should be as large as possible. When the autonomous device encounters a single activated obstacle, set When the autonomous device encounters multiple activated obstacles, the relative speed between the autonomous device and each activated obstacle is determined. direction; described The specific method for determining the direction is: definition Each element is the relative speed between the autonomous device and each activated obstacle, n is the number of activated obstacles, and a 2×n matrix A=[A1 A2] is defined. T ,in, Then, the matrix A is arranged from small to large according to the first row to obtain the matrix B. When the relative speed of the autonomous equipment and each activated obstacle is parallel, Same as when the autonomous device encounters a single activated obstacle, that is, when B(1, 1) = B(1, n), Otherwise, define a 2×n matrix C=[C1...C n-1 C n ],in And arrange the matrix C from large to small according to the second row to get the matrix D. At this time, 2. The collision avoidance method in a dynamic environment according to claim 1, characterized in that: The step 5) is specifically as follows: considering the maximum translation speed of the autonomous equipment to be U max , and Respectively In addition The two limit speeds that can be achieved after the offset speed in the direction, and For activated obstacles, and Respectively In addition The left and right limit speeds that can be achieved after the offset speed in the direction; Indicates that the autonomous equipment is from VO O The offset speed when avoiding collision on the left side, and is the corresponding relative speed, Indicates independent equipment from VO O The offset speed when avoiding collision on the right side, and is the corresponding relative speed, and It is obtained from the following conditions: Furthermore, considering the maximum translation speed U of the autonomous equipment max restrictions, and are constrained as follows: Therefore, you can get independent equipment from VO O Collision avoidance speed when performing collision avoidance on the left and right sides and 3. The method for avoiding collision in a dynamic environment according to claim 1, characterized in that: The collision avoidance rule is: when an obstacle needs to be avoided, no matter from which direction the obstacle approaches and whether the obstacle takes collision avoidance measures, the autonomous equipment will give priority to the VO of the obstacle. O Avoid on the right side.

4. The method for avoiding collision in a dynamic environment according to claim 3, characterized in that: The step 7) is specifically as follows: defining Each element represents the speed of each activated obstacle, defined as Each element represents the VO of the autonomous device from each obstacle. O The collision avoidance speed when avoiding collision on the left or right side is calculated by establishing the cost function J=J1+J2+J3 from V c0 Select the optimal collision avoidance speed.

5. The method for avoiding collision in a dynamic environment according to claim 4, characterized in that: In the cost function: the 1×2n matrix J1 is defined by the weight w1, where Define the 1×2n matrix J2 by ​​weight w2, where If the collision avoidance speed V c0 (i) It will put the autonomous device in the VO of other obstacles O The collision point where the collision is about to occur is (V c0 (i)-V O (i)) with the VO O The nearest intersection point of represents the position vector from the current position of the autonomous device to the collision point, and the collision avoidance reaction time is Otherwise c =w3, giving priority to a longer collision avoidance reaction time, and defining a 1×2n matrix J3 by weight w3, where J3(i)=w3-t c ; Therefore, define the 2×2n matrix E=[V c0 J] T , and the matrix E is arranged from small to large according to the second row to obtain the matrix F, the optimal collision avoidance speed is F(1, 1), and the autonomous equipment tracks the optimal collision avoidance speed until its state is updated to non-collision avoidance, thereby achieving the collision avoidance purpose.

Images