A trajectory planning method for unmanned surface vehicle in multi-ship encounter situation

Abstract

The application discloses a trajectory planning method for unmanned surface vehicles (USVs) in a multi-ship encounter situation, which is different from existing trajectory planning methods, and adopts overall thinking to simplify analysis of the encounter situation, namely, based on a consistent offset velocity direction (COVD) method, ships approaching from various directions of the USVs are regarded as one or two equivalent obstacles, and then a planning speed is designed by using a collision avoidance strategy conforming to COLREGS and kinematic constraints.

Description

TECHNICAL FIELD

[0001] The present application relates to a trajectory planning method for unmanned surface vehicles (USVs) in multi-ship encounter situations. BACKGROUND

[0002] Velocity obstacle (VO): This method maps obstacles to velocity space, forming a conic obstacle region. As long as the relative velocity vector of the USV and the obstacle is outside the VO, the USV and the obstacle will not collide. To facilitate the calculation of VO, we simplify the USV as a point and inflate the obstacle.

[0003] From the origin to the point of intersection of the line and the circle, the ray is represented as:

[0004] Minkowski sum:

[0005] Reflection:

[0006] As Figure 1 shown, a USV with shape A moving at velocity is given at position vector A barrier with shape B moving at velocity is given at position vector The safe distance between the USV boundary and the barrier boundary is d s . D is a circle with radius d s and The dangerous region of the barrier is Further, the dangerous region of the barrier with respect to the USV is Therefore, in the relative velocity space of the USV with respect to the barrier, the VO formed by the barrier can be represented as

[0007] Most of the existing trajectory planning algorithms only consider static obstacles or single dynamic obstacle in their design, and even some of them do not consider COLREGS. This limits their practical application range on the sea to some extent. Although there are some trajectory planning algorithms that can generate collision-free trajectories in accordance with COLREGS in the case of multiple ship encounters, they are based on the assumption that the traffic ships sail at a constant speed. In the real world, whether it is a manually operated ship or an automated ship, when encountering a collision danger, it will generally take certain collision avoidance measures. In order to make the collision-free trajectory planning method of the USV practical, in addition to assuming that the traffic ships have a certain level of intelligence, it is also necessary to consider the unexpected situation that these ships cannot take collision avoidance measures due to mechanical failure and other reasons. Therefore, these methods are difficult to be implemented in the real world. In addition, the existing trajectory planning methods that comply with COLREGS widely use relative bearing and relative heading to analyze the encounter situation, so as to assign the responsibility of keeping or giving way to the encountering ships. When multiple ships approach the USV from different directions at the same time, the encounter situation will become very complex and variable. Therefore, the calculation efficiency of these methods will be significantly reduced, and even these methods can hardly plan safe and effective collision-free trajectories for the USV. SUMMARY

[0008] The present application is directed to the problem of collision-free trajectory planning of USV in the case of multiple ship encounters. A new trajectory planning method is proposed. In the process of USV sailing to the target position, when encountering multiple traffic ships, whether the encountered ships comply with COLREGS or not, and regardless of the number of these ships and the number of different directions from which they approach the USV, the proposed trajectory planning method can quickly plan a collision-free trajectory for the USV in accordance with COLREGS. Unlike existing trajectory planning methods, the present application simplifies the analysis of the encounter situation by using a holistic thinking. That is, based on the consistent offset velocity direction (COVD) method, the ships approaching from different directions of the USV are regarded as one or two equivalent obstacles. Then, the planning speed is designed by using the proposed collision avoidance strategy in accordance with COLREGS and kinematic constraints.

[0009] The present application can be implemented by the following technical solutions:

[0010] A trajectory planning method for unmanned surface vehicle (USV) in the case of multiple ship encounters, comprising the following steps:

[0011] 1) Designing the desired speed;

[0012] 2) Collision risk assessment;

[0013] 3) Updating the state of the USV;

[0014] 4) Determining the offset velocity direction by the COVD method;

[0015] 5) Simplify all activated obstacles into one or two equivalent obstacles by equivalent obstacle method;

[0016] 6) Adopt collision avoidance strategy in accordance with COLREGS;

[0017] 7) Design collision avoidance speed;

[0018] 8) Design planning speed and next time's trajectory point.

[0019] The present application has the following beneficial effects:

[0020] 1) The equivalent obstacle method proposed simplifies the encountered vessels into one or two equivalent obstacles by adopting overall thinking, thereby simplifying the analysis of the encounter situation. No matter how complex the multi-ship encounter situation is, the present application can be applied and has high calculation efficiency.

[0021] 2) The collision avoidance strategy in accordance with COLREGS proposed is suitable for application to the situations where the encountered vessels comply with and do not comply with COLREGS, has wide application range and is practical. BRIEF DESCRIPTION OF DRAWINGS

[0022] Figure 1 It is an illustration of obstacle expansion and speed obstacle;

[0023] Figure 2 It is a flow chart of the trajectory planning method proposed by the present application;

[0024] Figure 3 It is an illustration of collision risk assessment;

[0025] Figure 4 It is an illustration of the direction of the designed bias speed;

[0026] Figure 5 It is an illustration of all activated obstacles being divided into two parts, front obstacles and rear obstacles;

[0027] Figure 6 It is an illustration of the activated obstacles being mapped onto the OD coordinate axis;

[0028] Figure 7 It is an illustration of all activated obstacles being simplified into one or two equivalent obstacles by equivalent obstacle method;

[0029] Figure 8 It is an illustration of the situation S=0 when the USV needs to avoid the front equivalent obstacle EO f and the rear equivalent obstacle EO r at the same time. DETAILED DESCRIPTION

[0030] The present application will be described in greater detail by way of specific embodiments, from which its advantages and effects will be apparent to those skilled in the art.

[0031] The flow chart of the trajectory planning method according to the present application is shown in Figure 2 , and the specific steps are as follows:

[0032] 1) Design the desired speed. According to the target position and current motion information of the USV, the desired speed of the USV is designed by using the constant bearing guidance wherein represents the target position of the USV. U d represents the desired approach speed when driving to the target position.

[0033]

[0034] wherein U dmax represents the maximum approach speed when driving to the target position. k p > 0 affects the deceleration behavior of the USV when it is about to reach the target position;

[0035] 2) Collision risk assessment. The collision risk assessment algorithm is executed in the velocity space and uses velocity obstacles. In order to facilitate the collision avoidance process, the obstacles detected by the USV in real time are classified according to the sensor data: activated obstacles and non-activated obstacles. The obstacles that have a collision risk with the USV are marked as activated, and the other obstacles are marked as non-activated.

[0036] The desired relative speed of the USV with respect to the obstacle and the actual relative speed are represented as:

[0037]

[0038] As shown in Figure 3 (the circles in the figure represent the inflated obstacles. and are the position vectors from the USV to the left and right tangent points of the inflated obstacle, respectively. represents the position vector of the USV from the current position along to the boundary of the inflated obstacle), when , it means that the USV driving at the desired speed will collide with the inflated obstacle sooner or later. When , it means that the USV driving at the current speed may collide with the obstacle in the short term. At this time, the collision avoidance reaction time t a needs to be calculated. wherein Indicates that the USV moves from its current position along... The position vector to the boundary of the inflated obstacle. And through the threshold t. r This is used to determine whether there is a risk of collision between the USV and the obstacle in the short term. In other words, the obstacle is marked as active when the following conditions are met:

[0039] or

[0040] 3) Update the USV's status. Update the USV's status based on all activated obstacle information. If an activated obstacle exists, the USV is in collision avoidance mode and enters the collision avoidance procedure, executing steps 4) to 8). Otherwise, it is in non-collision avoidance mode and executes step 8).

[0041] 4) Determine the bias velocity direction using the COVD method. The collision avoidance process only needs to consider activated obstacles. The collision avoidance velocity is designed by adding a bias velocity to the USV's desired velocity, allowing the USV to approach the target position while avoiding obstacles. Assume the number of activated obstacles is n, and n is a non-negative real number. i Represents the activated obstacle numbered i, where i ≠ 0 and i = 1, 2, ..., n. Indicates USV relative to O i The expected relative velocity. OD This represents a straight line parallel to the bias velocity vector. For example... Figure 4 As shown in (a), when When the moving USV is considered as a reference, obstacle O is activated. i by Approaching from all directions around the USV. That is... Figure 4 As shown in (b), when the USV is When moving, the USV will simultaneously approach each activated obstacle at different speeds. The USV needs to find a suitable offset velocity direction to give it sufficient safe maneuverability to avoid all activated obstacles at the same time. That is, l OD and The minimum included angle between them should be as large as possible.

[0042] θ = {θ1, θ2, ..., θ} 2n ) represents a set The set of angles between adjacent vectors in a given line l. i Bisect θ i In order to choose the appropriate l OD Design cost function J OD (l i )=θ i +wβ i β, where w is the weight.i It is l i and The included angle between them. And l OD Designed for

[0043] 5) Use the equivalent obstacle method to simplify all activated obstacles into one or two equivalent obstacles. Indicates the relationship with l oD Parallel unit vectors, whose direction points to... On the right side. For example... Figure 5 As shown, all activated obstacles are represented by l OD The boundary is divided into two parts: an obstacle in front and an obstacle behind. The condition is met. Activated obstacles are marked as obstacles in front, and other activated obstacles are marked as obstacles behind. And O i The category is D Oi express.

[0044]

[0045] Establish the OD coordinate axis, with its positive direction perpendicular to... The directions are the same. To facilitate the design of the USV's offset velocity during collision avoidance, both the obstacles in front and behind are mapped onto the OD coordinate axis (e.g., ...). Figure 6 (as shown in a and 6b). As... Figure 6 As shown, it can avoid O i Critical bias speed and It is calculated based on the following conditions.

[0046] and

[0047] and and The mapping on the OD coordinate axis can be represented as and Therefore, O i The mapping on the OD coordinate axis can be represented as in and They represent the range OD respectively Oi The minimum and maximum values ​​in, i.e.

[0048]

[0049] To simplify the analysis of the encountered situation, such as Figure 7 As shown in (a), mapping all obstacles in front onto the OD coordinate axis simplifies them into equivalent obstacles in front. in and EO f The minimum and maximum values ​​in.

[0050]

[0051] in This represents the mapping of obstacles ahead onto the OD coordinate axis, and

[0052]

[0053] Similarly, as Figure 7 As shown in (b), all rear obstacles are mapped onto the OD coordinate axis and simplified into equivalent rear obstacles. in and EO r The minimum and maximum values ​​in.

[0054] and

[0055]

[0056] in This represents the mapping of obstacles behind the object onto the OD coordinate axis, and

[0057]

[0058] Therefore, the simplified encounter scenarios using the equivalent obstacle method can be categorized into the following three types:

[0059] 1. When encountering situation S=1. At this time, only the equivalent obstacle EO in front exists. f Bias speed The design needs to meet

[0060] 2. When encountering situation S = -1. In this case, only the equivalent obstacle EO exists behind. r Bias speed The design needs to meet

[0061] 3. When encountering situation S=0. At this time, the equivalent obstacle in front EOf and the equivalent obstacle behind EO... r Both exist, bias speed The design needs to meet

[0062] 6) Adopt a collision avoidance strategy compliant with COLREGS. Based on COLREGS Rules 13-17, and considering the maritime practice of motor vessels overtaking on the starboard side of traffic vessels, as well as situations where traffic vessels do not comply with COLREGS, the following assumptions are made regarding the collision avoidance strategy of a USV in the event of a single-vessel encounter: a single activated obstacle Oi When approaching the USV from any direction, the USV will... The USV avoids it on its right. However, when multiple activated obstacles approach the USV simultaneously, the USV may not be able to simultaneously satisfy the COLREGS with each activated obstacle. In this case, the USV prioritizes avoiding one of the obstacles. Next, the above assumptions are applied to the proposed equivalent obstacle method:

[0063] 1. Encounter situation S=1. The USV passes all obstacles ahead from its port side. To minimize the deviation from the desired trajectory during collision avoidance, the offset speed is... Designed for

[0064]

[0065] 2. Encounter situation S=-1. The USV passes all rear obstacles on its starboard side. To minimize the deviation from the desired trajectory during collision avoidance, the offset speed is... Designed for

[0066]

[0067] 3. When encountering situation S=0, two situations may occur:

[0068] (c) such as Figure 8 As shown in (a), when At that time, the USV cannot simultaneously satisfy the COLREGS with all activated obstacles. The USV prioritizes turning to starboard to pass all activated obstacles on its port side. To reduce the deviation from the desired trajectory during collision avoidance, the offset speed is... Designed for

[0069] (d) such as Figure 8 As shown in (b), when At that time, the USV passes all obstacles in front of it from its port side and all obstacles behind it from its starboard side. To minimize deviation from the desired trajectory during collision avoidance, the offset speed is... Designed for

[0070] Figure 8 When the encounter situation S=0, the USV needs to simultaneously avoid the equivalent obstacle EO ahead. f and rear equivalent barrier EO r For ease of display, EO f and EO r Displayed in the same analytical graph;

[0071] 7) Design the collision avoidance speed. The collision avoidance speed obtained based on the proposed equivalent obstacle method and collision avoidance strategy is:

[0072] 8) Design the planning velocity and the next time's trajectory point. According to the current state of the USV, the desired planning velocity of the USV is designed as

[0073] Although the current velocity of the USV cannot be switched to But it can gradually converge to In order to make the planned trajectory of the USV more practical and smooth, the planning heading p And the planning velocity size V p The following kinematic constraints are made.

[0074]

[0075] Where R mmin And R mmax The current minimum heading and maximum heading relative to the current heading min And RV max The current minimum translation speed and maximum translation speed relative to the current translation speed V. And

[0076]

[0077] Where W min And W max The minimum rotation speed and the maximum rotation speed.a Vmin And a Vmax The minimum translation acceleration and the maximum translation acceleration.t Δ The sampling time interval. Therefore, the designed planning heading and planning velocity size are represented as:

[0078]

[0079]

[0080] And the designed planning velocity is

[0081] Further, the next time's planning trajectory point is

[0082] The above only describes the preferred embodiments of the present application and is not used to limit the present application. Any modification, equivalent replacement and improvement made within the spirit and principle of the present application shall be included in the protection scope of the present application.

Claims

1. A trajectory planning method for unmanned surface vessels (USVs) in multi-vessel encounter scenarios, characterized in that, Includes the following steps: 1) Design desired speed; 2) Collision risk assessment; 3) Update the USV status; 4) Determine the direction of the bias velocity using the COVD method; 5) Use the equivalent obstacle method to simplify all activated obstacles into one or two equivalent obstacles; 6) Adopt a collision avoidance strategy that conforms to COLREGS; 7) Design collision avoidance speed; 8) Design and plan the speed and trajectory point at the next moment; step 5) specifically includes: Indicates the relationship with l OD Parallel unit vectors, l OD Represents: a straight line parallel to the bias velocity vector. Its direction points to On the right side, Indicates: the desired velocity of the USV using a constant azimuth guidance design, with all activated obstacles at l OD The boundary is divided into two parts: a front obstacle and a rear obstacle, satisfying the condition. Activated obstacles are marked as obstacles in front, and other activated obstacles are marked as obstacles behind, and O i The category is D Oi express, Indicates USV relative to O i Expected relative speed, O i This represents the activated obstacle numbered i, where i ≠ 0 and i = 1, 2, ..., n. Establish an OD coordinate axis, with its positive direction perpendicular to... With the same direction, to facilitate the design of the USV's offset velocity during collision avoidance, both the obstacles in front and behind are mapped onto the OD coordinate axis, enabling avoidance of O-axis collisions. i Critical bias speed and It is calculated based on the following conditions: and and The mapping on the OD coordinate axis can be represented as and Therefore, O i The mapping on the OD coordinate axis can be represented as in and They represent the range OD respectively Oi The minimum and maximum values ​​in, i.e.

2. The trajectory planning method for an unmanned surface vessel in a multi-ship encounter situation according to claim 1, characterized in that, Step 1) specifically includes: based on the target position and current motion information of the USV, designing the desired velocity of the USV using invariant orientation guidance. in Indicates the target location of the USV, U d This indicates the expected approach speed when moving towards the target location. U dmax k represents the maximum approach speed when moving towards the target location. p >0 affects the deceleration behavior of the USV as it approaches the target location.

3. The trajectory planning method for an unmanned surface vessel in a multi-vessel encounter situation according to claim 1, characterized in that, Step 2) specifically refers to: 1) For each obstacle detected by the USV in real time, a collision risk assessment is performed simultaneously. This collision risk assessment algorithm is executed in the velocity space and uses velocity barriers. Based on the sensor data, the obstacles detected by the USV in real time are classified into: active obstacles and inactive obstacles. Obstacles that have a collision risk with the USV are marked as active, and other obstacles are marked as inactive. USV's expected relative speed to the obstacle and actual relative speed Represented as: when This means that a USV traveling at the desired speed will eventually collide with the expanded obstacle; when This means that the USV traveling at its current speed may collide with an obstacle in the short term, at which point the collision avoidance reaction time t needs to be calculated. a , in Indicates that the USV moves from its current position along... The position vector to the boundary of the inflated obstacle, and through the threshold t r To determine whether there is a risk of collision between the USV and the obstacle in the short term, the obstacle is marked as active when the following conditions are met. or 4. The trajectory planning method for an unmanned surface vessel in a multi-vessel encounter situation according to claim 1, characterized in that, In step 3), the USV's state is updated based on all activated obstacle information. As long as there is an activated obstacle, the USV is in collision avoidance state and enters the collision avoidance procedure to execute steps 4) to 8). Otherwise, it is in non-collision avoidance state and executes step 8).

5. The trajectory planning method for an unmanned surface vessel in a multi-vessel encounter situation according to claim 1, characterized in that, Step 4) specifically refers to: Assuming the number of activated obstacles is n, and n is a non-negative real number, O(n) i This represents the activated obstacle numbered i, where i ≠ 0 and i = 1, 2, ..., n. Indicates USV relative to O i The expected relative velocity, l OD Represents a straight line parallel to the bias velocity vector, when... When the moving USV is considered as a reference, obstacle O is activated. i by Approaching from all directions around the USV, when the USV... When moving, the USV will simultaneously approach each activated obstacle at different speeds. The USV needs to find a suitable offset velocity direction to give it sufficient safe maneuverability to simultaneously avoid all activated obstacles. OD and The minimum included angle between them should be as large as possible. θ = {θ1, θ2, ..., θ} 2n } represents a set The set of angles between adjacent vectors, and the line l i Bisect θ i In order to choose the right l OD Design cost function J OD (l i )=θ i +wβ i , Where w is the weight, β i It is l i and The included angle between them, and l OD Designed for 6. The trajectory planning method for an unmanned surface vessel in a multi-vessel encounter situation according to claim 1, characterized in that, Step 6) specifically refers to: When encountering situation S=1, the USV passes all obstacles ahead from its port side. In order to minimize the deviation from the desired trajectory during collision avoidance, Bias speed Designed for When encountering a situation S=-1, the USV passes all rear obstacles from its starboard side. To minimize the deviation from the desired trajectory during collision avoidance, the offset speed is... Designed for When the situation S=0, two situations may occur: (a) When At that time, the USV cannot simultaneously satisfy the COLREGS with each of the activated obstacles. The USV prioritizes turning to starboard to pass all activated obstacles on its port side, and in order to minimize deviation from the desired trajectory during collision avoidance, it adjusts its offset speed. Designed for (b) when At that time, the USV passes all obstacles in front of it from its port side and all obstacles behind it from its starboard side. In order to reduce the deviation from the desired trajectory during collision avoidance, the offset speed is... Designed for To simplify the analysis of the encounter situation, all obstacles in front are mapped onto the OD coordinate axis and simplified into equivalent obstacles in front. in and EO f The minimum and maximum values ​​in, and Among them OD fOi This represents the mapping of obstacles ahead onto the OD coordinate axis, and Mapping all rear obstacles onto the OD coordinate axis simplifies them into equivalent rear obstacles in and EO r The minimum and maximum values ​​in, and Among them OD rOi This represents the mapping of obstacles behind the object onto the OD coordinate axis, and 7. The trajectory planning method for an unmanned surface vessel in a multi-vessel encounter situation according to claim 6, characterized in that, Step 7) specifically involves: obtaining the collision avoidance speed based on the proposed equivalent obstacle method and the collision avoidance strategy conforming to COLREGS.

8. The trajectory planning method for an unmanned surface vessel in a multi-ship encounter situation according to claim 7, characterized in that, Step 8) specifically involves: Based on the current state of the USV, the expected planned speed of the USV is designed as follows: Although the current speed of the USV cannot be switched immediately However, it can gradually converge to this through heading and speed control. To make the planned trajectory of the USV more realistic and smooth, the planned heading Ψ p And the planned speed size V p Apply the following kinematic constraints. Among them RΨ min and RΨ max RV represents the minimum and maximum achievable headings relative to the current heading Ψ. min and RV max These are the minimum and maximum achievable translation speeds relative to the current translation speed V, and... Among them W min and W max These are the minimum and maximum rotational speeds, a Vmin and a Vmax These are the minimum and maximum translational accelerations, t Δ The sampling time interval is used; therefore, the designed planned course and planned speed are expressed as follows: And the designed planning speed is Furthermore, the planned trajectory point for the next moment is...

Images