Unmanned surface vehicle local path planning method based on improved dynamic window method

Through the improved dynamic window method, combined with international maritime collision avoidance rules and adaptive strategies, the unmanned boat path planning is optimized, and the dynamic window method deviates from the route and follows the rules in a multi-obstruction environment, realizing the unmanned boat's Safe and efficient avoidance.

CN119045494APending Publication Date: 2024-11-29HARBIN UNIV OF SCI & TECH
View PDF 0 Cites 5 Cited by

Patent Information

Application Number
CN202411163681.3
Authority / Receiving Office
CN · China
Patent Type
Applications(China)
Current Assignee / Owner
Filing Date
2024-08-23
Publication Date
2024-11-29

AI Technical Summary

Technical Problem

The existing dynamic window method lacks flexibility in unmanned boat path planning, making it difficult to effectively avoid multiple dynamic obstacles and comply with international maritime collision avoidance rules, resulting in unmanned boats that may deviate from the route or be unable to complete the resumption of navigation tasks.

Method used

The improved dynamic window method is adopted, combined with international maritime collision avoidance rules, and through segmented fusion strategies and adaptive velocity weight coefficients, the global evaluation component and steering angular velocity evaluation factor are introduced, and the trajectory evaluation function is optimized to ensure that unmanned boats encounter situations in different situations. and safe avoidance in mixed scenarios.

Benefits of technology

It significantly improves the safety, feasibility and adaptability of unmanned boat path planning, ensures that obstacles can be effectively avoided and collision avoidance rules can be followed in complex maritime environments, and achieves efficient dynamic collision avoidance tasks.

✦ Generated by Eureka AI based on patent content.

Smart Images

  • Figure CN119045494A_ABST
    Figure CN119045494A_ABST
Patent Text Reader

Abstract

The invention discloses an unmanned surface vehicle local path planning method based on an improved dynamic window method, which belongs to the field of unmanned surface vehicle path planning, and comprises the following steps: firstly, initializing a grid map matrix according to known environment information, and then aiming at the local optimum problem of the traditional dynamic window method, calculating the local path of the unmanned surface vehicle. A local optimal trajectory is improved by adopting a segmented fusion strategy, a trajectory evaluation function is comprehensively optimized and improved by introducing a self-adaptive speed weight coefficient, a global evaluation component and a steering angular speed evaluation factor, and requirements on collision avoidance action, avoidance amplitude, re-flight opportunity and the like are comprehensively considered in combination with an international maritime collision avoidance rule at the same time. According to the method, collision avoidance simulation under four meeting situations such as meeting, tracking, right crossing and left crossing is compared and analyzed, and the feasibility and effectiveness of the planning method in a mixed scene are verified.
Need to check novelty before this filing date? Find Prior Art

Description

Technical Field

[0001] The invention belongs to the technical field of unmanned boat path planning, and in particular relates to a local path planning method for a surface unmanned boat based on an improved dynamic window method. Background Art

[0002] With the development of information and communication technology and artificial intelligence technology, unmanned boats with advantages such as unmanned, intelligent, low-cost and high efficiency will occupy an important position in the future marine development and intelligent equipment applications. The path planning problem of unmanned boats has always been one of the research hotspots in the process of autonomous navigation of ships, and the difficulty and challenge of realizing this problem mainly lies in the design of intelligent collision avoidance algorithms. Due to the complex and changeable sea environment, not only static obstacles will be encountered during navigation, including irregular reefs and other obstacles, but also unpredictable dynamic obstacles such as floating objects, marine garbage, etc., especially other ships in motion, which puts higher requirements on the comprehensive ability of the algorithm. Therefore, when planning the path of unmanned boats, it is necessary not only to coordinate the overall and local, but also to ensure that the designed collision avoidance algorithm is safe and reliable to cope with the real-time changing marine environment.

[0003] The dynamic window method is a local planning method based on speed, which transforms the path planning problem into a constrained optimization problem in the speed vector space. The basic idea is to define a dynamic window in the speed space of linear speed and angular speed, and the window represents the speed range that the unmanned boat can choose. According to the preset prediction simulation time period, the speed space in the window area is sampled, and multiple sets of data in one cycle are used to simulate the motion trajectory of the unmanned boat. Then, the designed evaluation function is used to evaluate the safety and quality of each linear speed and angular speed combination in the window, and finally the ideal trajectory speed is selected to control the unmanned boat to perform the moving operation, and then the state information of the next moment is updated repeatedly to complete the local route planning.

[0004] However, the current dynamic window method only considers the local optimal trajectory in the process of guiding the movement of mobile robots such as unmanned boats. When in an unknown local environment, the unmanned boat will give priority to safety when avoiding dynamic or suddenly appearing unknown obstacles. At this time, the unmanned boat will only move according to the optimal trajectory at the current moment. If multiple dynamic obstacles successively form an intersection or encounter situation with it, this may cause the unmanned boat to deviate from the route and fail to complete the task of re-entering the route. In addition, the traditional algorithm still uses constant weight parameters, lacks the flexibility of obstacle avoidance, and does not or less considers the constraints of the International Maritime Collision Prevention Regulations. Therefore, around the problem of local path planning of unmanned boats, it is necessary to fully consider factors such as collision avoidance rules, kinematic characteristics of unmanned boats and complexity of maritime traffic to make it more in line with the actual collision avoidance situation of unmanned boats. Summary of the invention

[0005] The purpose of the present invention is to provide a local path planning method for an unmanned surface vehicle based on an improved dynamic window method to solve the problems raised in the above-mentioned background technology.

[0006] To achieve the above object, the present invention provides the following technical solution: a method for local path planning of an unmanned surface vehicle based on an improved dynamic window method, comprising the following steps:

[0007] Step 1: Initialize the environmental information, analyze the local path planning problem in combination with the International Regulations for Avoiding Collisions at Sea, and initialize the grid map matrix based on the known information in the current environment, including the starting point and destination information of the unmanned boat and the dynamic and static obstacle information;

[0008] Step 2: The dynamic window method obtains the corresponding trajectory within the prediction simulation period according to the motion model parameters of the unmanned boat, and establishes the motion model of the unmanned boat. The velocity space of the unmanned boat is determined according to the established motion model and motion constraints. The position and posture of the unmanned boat in ship kinematics are expressed by vector Indicates that x t and t They represent the coordinates of the unmanned boat at time t in the coordinate system, It represents the heading angle of the unmanned boat at time t. In each sampling period, the motion trajectory of the unmanned boat is approximated, and the motion path in the sampling period is regarded as a straight line. The displacement and angle relationship of the unmanned boat at time t+1 relative to time t in the geodetic coordinate system is: Among them, v t ,ω t are the linear velocity and angular velocity of the unmanned boat at time t, respectively, and Δt is the unit sampling time;

[0009] Step 3: Discretely sample the linear velocity and angular velocity in the velocity vector space to simulate the motion trajectory of the unmanned boat, and use the evaluation function to evaluate each speed combination;

[0010] The ideal trajectory speed is selected to control the unmanned boat to perform the moving operation, where the evaluation function G(v,ω) is expressed as: G(v,ω)=σ(α·heading(v,ω)+β·dist(v,ω)+γ·velocity(v,ω)), where heading(v,ω) represents the azimuth evaluation subfunction, dist(v,ω) represents the safety distance evaluation subfunction, velocity(v,ω) represents the speed evaluation subfunction, α is the weight of the target azimuth, β is the weight of the safety distance from the obstacle, γ is the weight of the speed, and σ is the normalization function;

[0011] Step 4: Optimize and improve the traditional dynamic window algorithm, use segmented fusion strategy to improve the local optimal trajectory, and integrate collision avoidance rules to comprehensively improve the trajectory evaluation function to achieve safe avoidance in different encounter situations and mixed scenarios;

[0012] Before using the local path planning algorithm for dynamic obstacle avoidance, all nodes on the path are segmented and screened, and the key points on the global path are tracked for movement. The adaptive speed weight coefficient τ is introduced to dynamically adjust the speed evaluation subfunction. The expression of τ is:

[0013]

[0014] Where c1 and c2 are parameters between [0,1], dist min is the minimum distance between the ship trajectory and the obstacle, R obs Indicates the distance threshold for conflict determination.

[0015] Preferably, in step one, when the surface unmanned boat is sailing in an unknown environment, in addition to achieving safe avoidance of dynamic and static obstacles, it is also necessary to comply with the international maritime collision avoidance regulations. Therefore, when the unmanned boat and the obstacle ship form different encounter situations, actions that comply with the collision avoidance rules should be taken. In order to integrate the collision avoidance rules into the collision avoidance method of the unmanned boat, the common encounter situations are divided into four types: encounter, right crossing, left crossing and overtaking. Since the unmanned boat has a small hull and strong maneuverability, in the actual encounter situation, it will be safer for the unmanned boat to perform evasive actions as the active avoider. Therefore, when the unmanned boat and the obstacle have the above four encounter situations, the unmanned boat is a way-giving ship, and other obstacle ships are defined as straight-moving ships.

[0016] Preferably, in step 2, by continuously updating v t and ω t , the position and heading angle of the unmanned boat in the entire simulation cycle can be calculated. The dynamic window method also considers the dynamic constraints of the unmanned boat, including the maximum and minimum speed constraints V s , Motor acceleration and deceleration constraints V d and braking distance constraint V SD , so the final feasible velocity space set can be defined as V r =V s ∩V d ∩V SD .

[0017] Preferably, in step 4, since the evaluation function in the traditional algorithm does not take into account the global factor information, in order to better utilize the global path to guide the navigation of the unmanned boat, by introducing the global evaluation component dist SG (v,ω) improves the evaluation function, the global evaluation component dist SGThe calculation formula for (v,ω) is:

[0018] Preferably, where dist ST Indicates the distance between the starting point of the global path and the end of the current predicted trajectory of the unmanned boat, dist TG Indicates the distance between the end of the current predicted trajectory of the unmanned boat and the end of the global path.

[0019] Preferably, the steering angular velocity evaluation factor steering(v,ω) is added to the evaluation function to make the track conform to the kinematic characteristics of the unmanned boat as much as possible, thereby improving the seaworthiness, and the calculation formula of the steering angular velocity evaluation component steering(v,ω) is: steering(v,ω)=(|ω max |-|ω·Δt|) / |ω max |.

[0020] Preferably, where ω max is the maximum angular velocity of the unmanned boat, and ω is the turning angular velocity at the current moment.

[0021] Preferably, the collision avoidance rule is integrated into the evaluation function of the dynamic window method to ensure that the selected velocity vector and the formed collision avoidance path meet the requirements of the collision avoidance rule. The final improved trajectory evaluation function can be expressed as:

[0022]

[0023] Preferably, rules(v,ω) is used to calculate the distance for the unmanned boat to take evasive action, as well as to determine the turning direction, turning amplitude and timing of re-entry. Nheading(v,ω) is represented by the new azimuth evaluation subfunction after the key point is introduced, τ is the speed dynamic evaluation weight coefficient, steering(v,ω) represents the steering angular velocity evaluation subfunction, and dist SG (v,ω) represents the global evaluation subfunction, R safe is the safe distance for encounter situations, D is the relative distance between the two ships in the longitudinal direction, μ, η and ε correspond to steering(v,ω), dist SG The weight coefficients of the three components of (v, ω) and rules(v, ω) can be dynamically adjusted to allow the collision avoidance rules to be better integrated into the dynamic window method, so that the appropriate velocity vector can be flexibly selected according to the actual sea conditions to complete the collision avoidance operation.

[0024] Compared with the prior art, the present invention has the following beneficial effects:

[0025] (1) The present invention provides a local path planning method for a surface unmanned boat based on an improved dynamic window method. Combined with the kinematic characteristics of the unmanned boat, the present invention fully considers the requirements of the International Regulations for Preventing Collisions at Sea regarding early, large, wide and clear avoidance. It verifies that the present method is significantly superior to the traditional dynamic window method in terms of real-time performance and safety, and has certain practical significance.

[0026] (2) The present invention provides a local path planning method for a surface unmanned boat based on an improved dynamic window method. Different from the traditional dynamic window method which has the disadvantages of lack of adaptability and easy to fall into local optimum, the present method adopts a segmented fusion strategy to effectively avoid the problem of local optimal trajectory. By introducing adaptive speed weight coefficient, global evaluation component and steering angular velocity evaluation factor, the trajectory evaluation function is comprehensively optimized and improved, thereby improving the safety and feasibility of the unmanned boat track planning and ensuring that it can efficiently complete the dynamic collision avoidance task. BRIEF DESCRIPTION OF THE DRAWINGS

[0027] Figure 1 A flowchart of an improved dynamic window local path planning method of the present invention;

[0028] Figure 2 It is a schematic diagram of the encounter situation division of the present invention;

[0029] Figure 3 It is a schematic diagram of the segmented fusion of the track of the unmanned boat at a certain stage of the present invention;

[0030] Figure 4 It is a simulation schematic diagram of the avoidance process using the traditional dynamic window method in the encounter situation of the present invention;

[0031] Figure 5 It is a simulation schematic diagram of the avoidance process using the improved dynamic window method in the encounter situation of the present invention;

[0032] Figure 6 It is a simulation schematic diagram of the avoidance process using the traditional dynamic window method in the overtaking situation of the present invention;

[0033] Figure 7 It is a simulation schematic diagram of the avoidance process using the improved dynamic window method in the overtaking situation of the present invention;

[0034] Figure 8 It is a simulation schematic diagram of the avoidance process using the traditional dynamic window method in the right crossing situation of the present invention;

[0035] Fig. 9 It is a simulation schematic diagram of the avoidance process using the improved dynamic window method in the right crossing situation of the present invention;

[0036] Fig.10 It is a simulation schematic diagram of the avoidance process using the traditional dynamic window method in the left crossing situation of the present invention;

[0037] Fig.11 It is a simulation schematic diagram of the avoidance process using the improved dynamic window method in the left crossing situation of the present invention;

[0038] Fig.12 It is a schematic diagram of the simulation of the avoidance process in the mixed scenario of the present invention. DETAILED DESCRIPTION

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

[0040] The present invention provides Figure 1-12 A technical solution shown: a local path planning method for a surface unmanned boat based on an improved dynamic window method, the process of the method is as follows Figure 1 As shown, the specific steps include:

[0041] Step 1: Initialize environmental information and analyze the local path planning problem in combination with the International Regulations for Avoiding Collisions at Sea;

[0042] The step 1 specifically includes:

[0043] Initialize the grid map and the environmental information. This mainly includes the starting point and destination information of the USV and the dynamic and static obstacle information. The dynamic obstacle information mainly includes the longitudinal speed, heading, current position, etc. of the obstacle ship. The static obstacle information mainly refers to its position and shape information. This algorithm assumes that the information of dynamic and static obstacles is known. This information can be obtained through AIS and radar during actual sea navigation.

[0044] When a surface unmanned boat is navigating in an unknown environment, it only knows the current position and the coordinates of the end point. It detects environmental information based on its own sensors, obtains the size, shape, direction of obstacles, and the moving speed of dynamic obstacles, makes local adjustments based on the obstacle information, avoids obstacles, and returns to the original route to continue sailing. In addition to achieving safe avoidance of dynamic and static obstacles during navigation, it is also necessary to comply with international maritime collision avoidance regulations. Therefore, when the unmanned boat and the obstacle ship form different encounter situations, actions that comply with the collision avoidance rules should be taken. In order to integrate the collision avoidance rules into the collision avoidance method of the unmanned boat, the common encounter situations are divided into four types: encounter, right crossing, left crossing, and overtaking. The schematic diagram of the encounter situation division is shown below. Figure 2 As shown, USV means own ship, and OBS means obstacle ship;

[0045] 1. Encounter: When the USV and the obstacle vessel are sailing in opposite or nearly opposite directions and there is a risk of collision, that is, the heading angle difference is satisfy The USV should turn right and pass the target ship on its port side.

[0046] 2. Left crossing: When the USV and the target ship cross each other and there is a risk of collision, if the heading angle is different satisfy This constitutes a port crossing. At this time, the USV is a straight-line ship and the target ship is a give-way ship.

[0047] 3. Right cross, if the heading angle is different satisfy This constitutes a starboard crossing. At this time, the target ship is the straight-line ship and the USV is the give-way ship.

[0048] 4. Overtaking: When the heading angle difference between the USV and the target ship is satisfy The USV should turn left and pass the target ship on its port side.

[0049] Since USVs are small and highly maneuverable, it is safer for USVs to take evasive action as the active avoider in actual encounter situations. Therefore, when USVs encounter obstacles in the above four situations, USVs are all give-way vessels, and other obstacle vessels are defined as straight-moving vessels.

[0050] Step 2: Establish a motion model of the unmanned boat, and determine the velocity space of the unmanned boat based on the established motion model and motion constraints;

[0051] The step 2 specifically includes:

[0052] Since the dynamic window method needs to obtain the corresponding trajectory within the prediction simulation period based on the motion model parameters of the unmanned boat, it is necessary to establish the unmanned boat motion model. In the dynamic window, each trajectory corresponds to a velocity pair (v, ω), where v represents the linear velocity of the unmanned boat, and ω represents the yaw angular velocity of the unmanned boat. The vector is used to define the position and posture of the unmanned boat in ship kinematics. Indicates that x t and t They represent the coordinates of the unmanned boat at time t in the geodetic coordinate system, It represents the heading angle of the unmanned boat at time t. In each sampling period, the motion trajectory of the unmanned boat is approximated, and the motion path in the sampling period is regarded as a straight line. The displacement and angle relationship of the unmanned boat at time t+1 relative to time t in the geodetic coordinate system is:

[0053]

[0054] Among them, vt ,ω t are the linear velocity and angular velocity of the unmanned boat at time t, respectively. Δt is the unit sampling time. Therefore, at the next moment, the unmanned boat moves linearly along the bow angle by v. t Δt, its heading angle rotates ω relative to the geodetic coordinate system t Δt. By continuously updating v t and ω t , the position and heading angle of the unmanned boat in the entire simulation cycle can be calculated;

[0055] The dynamic window method also considers the dynamic constraints of the USV (linear velocity, angular velocity, acceleration, etc.). In the actual process, the range of sampling speed needs to be limited according to the constraints of the USV and the environment;

[0056] 1. Maximum and minimum speed constraints of unmanned boats: The sampling speed should meet the maximum and minimum speeds that the unmanned boat can withstand, using V s Represents the set of the maximum speed and minimum speed of the unmanned boat, then:

[0057] V s ={(v,ω)|v∈[v min ,v max ]∧ω∈[ω min ,ω max ]} (4)

[0058] In formula (4), v min is the minimum linear speed of the unmanned boat, v max is the maximum linear speed of the unmanned boat, ω min is the minimum angular velocity of the unmanned boat, ω max is the maximum angular velocity of the unmanned boat.

[0059] 2. Unmanned boat motor acceleration and deceleration constraints: Considering the dynamic characteristics of the unmanned boat, its acceleration or deceleration rate is bounded. There is a maximum acceleration and maximum deceleration during the navigation of the unmanned boat. The actual speed range that can be achieved is V d for

[0060]

[0061] In formula (5), v c ,ω c is the current linear velocity and angular velocity of the unmanned boat, is the maximum deceleration and maximum acceleration of the unmanned boat linear speed, is the maximum deceleration and maximum acceleration of the angular velocity of the unmanned boat;

[0062] 3. Braking distance constraint of unmanned boat: In order to ensure that the unmanned boat can safely and effectively avoid obstacles when sailing in unknown waters, the braking distance constraint needs to be considered, that is, the forward speed at the next sampling time must satisfy the distance between the unmanned boat and the nearest obstacle greater than the braking distance SD. The calculation of the braking distance SD is shown in formula (6), where it is assumed that the forward speed of the unmanned boat can be successfully reduced to 0 after m sampling periods;

[0063]

[0064] Under the constraint of maximum acceleration, the velocity set V that can make the unmanned boat stop in time before colliding with an obstacle is SD for:

[0065]

[0066] In formula (7), D(v,ω) is the shortest distance between the current unmanned boat and the obstacle on the motion trajectory corresponding to the speed (v,ω);

[0067] In summary, under the combined influence of the above three constraints, the final feasible speed space set can be defined as V r , then:

[0068] V r =V s ∩V d ∩V SD (8)

[0069] Step 3: Discretely sample the linear velocity and angular velocity in the velocity vector space to simulate the motion trajectory of the unmanned boat, use the evaluation function to evaluate each speed combination, and select the ideal trajectory speed to control the unmanned boat to perform the moving operation;

[0070] The step three specifically includes:

[0071] For the local path planning of the unmanned boat, in the final speed space V r There are multiple sets of sampling speeds in , and these speed pairs can generate several predicted trajectories. Therefore, it is necessary to design an evaluation function to select the optimal trajectory. The target evaluation function of the dynamic window method consists of three weighted items, which consider the azimuth, safety distance and speed respectively, and score each trajectory based on these three evaluation indicators. The sampling speed corresponding to the path with the highest score is selected as the comprehensive optimal speed and executed. The trajectory evaluation function is as follows:

[0072] G(v,ω)=σ(α·heading(v,ω)+β·dist(v,ω)+γ·velocity(v,ω)) (9)

[0073] In formula (9), heading(v,ω) represents the azimuth evaluation subfunction, dist(v,ω) represents the safety distance evaluation subfunction, velocity(v,ω) represents the speed evaluation subfunction, α is the weight of the target azimuth, β is the weight of the safety distance from the obstacle, γ is the weight of the speed, and σ is the normalization function;

[0074] The three sub-functions in the evaluation function are introduced as follows:

[0075] 1. Azimuth evaluation subfunction:

[0076] This function is evaluated based on the angle between the speed direction at the end point of the current trajectory of the unmanned boat and the direction of the target point. Ideally, the unmanned boat is expected to always move towards the target point during navigation. The function is shown in the following formula:

[0077] heading(v,ω)=180°-θ (10)

[0078] In formula (10), θ is uniquely determined by the deviation between the actual heading angle of the unmanned boat at the end of the simulated trajectory and the expected heading angle. The smaller θ is, the higher the evaluation score is and the smaller the deviation from the destination is.

[0079] 2. Safety distance evaluation subfunction

[0080] This function represents the minimum distance between the end of the predicted trajectory and the obstacle. When there are obstacles around the unmanned boat, it traverses the set of dynamic, static and unknown obstacles, and obtains the nearest obstacle distance by calculation and comparison. obsmin , if there is no obstacle close to the unmanned boat, and even the nearest obstacle is very far away, then the minimum distance at this time may be too large. In order to avoid this situation, a maximum threshold C should be set to ensure that the safety distance evaluation sub-function does not affect the entire evaluation function. The function is shown in the following formula:

[0081]

[0082] In formula (11), distance obsmin It represents the minimum distance between the unmanned boat and the obstacle, and C is the set maximum threshold.

[0083] 3. Speed ​​evaluation subfunction

[0084] This function represents the forward speed of the unmanned boat. That is, under the premise of safe collision avoidance, when it is close to an obstacle, the unmanned boat will slow down. Otherwise, the unmanned boat will speed up to achieve the purpose of quickly reaching the target position. The function is shown in the following formula:

[0085] velocity(v,ω)=abs(v) (12)

[0086] In different environments, the weights of the azimuth evaluation function, safety distance evaluation function, and speed evaluation function are different. Therefore, the above evaluation functions need to be normalized to between [0,1] before calculation to avoid any value being too large. The specific calculation formula is as follows:

[0087]

[0088] Among them, n is the number of trajectories generated according to the speed pair (v, ω) and resolution calculation within the prediction simulation cycle, and i is the speed combination sequence number of the currently selected evaluation trajectory. According to the above conditions, selecting an optimal trajectory means that the trajectory should be as far away from obstacles as possible to ensure safety, while minimizing the azimuth angle with the target and being able to reach the target point quickly. Evaluating this trajectory will obtain the highest evaluation value.

[0089] Step 4: Optimize and improve the traditional dynamic window algorithm, adopt a segmented fusion strategy to improve the local optimal trajectory, and integrate the collision avoidance rules to comprehensively improve the trajectory evaluation function to achieve safe avoidance in different encounter situations and mixed scenarios.

[0090] The step 4 specifically includes:

[0091] The traditional dynamic window method only considers the local optimal trajectory in the process of guiding the movement of the unmanned boat. When passing through the U-shaped sea area, it is easy to cause deadlock, thus falling into the local optimum. In addition, the marine environment is complex and changeable. When in an unknown local environment, the unmanned boat will give priority to safety when avoiding dynamic or sudden unknown obstacles, that is, it must not collide with obstacles. After the local collision avoidance is completed, the unmanned boat will return to the global path planning, which will greatly increase the navigation distance. Even when multiple dynamic obstacles successively form an intersection or encounter situation with it, this may cause the unmanned boat to deviate from the route and fail to complete the re-navigation task. Therefore, before using the local path planning algorithm for dynamic obstacle avoidance, first use the known information to obtain a safe and collision-free global path, and then traverse all the nodes of the path. If the current node is on the same straight line as the previous two nodes, then the previous node can be regarded as a redundant node and deleted, and the selected node will be used as the local sub-target point of the dynamic window method;

[0092] At this time, the dynamic window rule does not need to perform blind search, but only needs to track the key points on the global path to move. The schematic diagram of the segmented fusion of the unmanned boat's track at a certain stage is as follows: Figure 3 As shown, based on this improvement strategy, the local optimal trajectory problem of the dynamic window algorithm can be effectively improved.

[0093] In the traditional dynamic window method, the trajectory evaluation function G(v,ω) consists of three components: azimuth, safety distance and speed. The corresponding weight coefficients α, β and γ play a decisive role in the selection of the optimal trajectory. Usually, the weight coefficients are fixed and cannot meet the needs of ship path planning in complex and changing environments.

[0094] Therefore, it is critical to select the optimal weight coefficient to ensure that the ship can obtain the best path in various environments. In order to improve the adaptability of the traditional dynamic window algorithm planning path, the adaptive speed weight coefficient τ is introduced to dynamically adjust the speed evaluation subfunction. The expression of τ is:

[0095]

[0096] In formula (16), c1 and c2 are parameters between [0,1], dist min is the minimum distance between the ship trajectory and the obstacle, R obs It represents the distance threshold for conflict judgment. By dynamically adjusting the weight coefficient of the speed component, it can avoid the forward speed being too small or too large due to a fixed value, so as to better achieve the best balance of speed and enable the ship to reach the target point as quickly as possible.

[0097] During the driving process of the unmanned boat, after the global key node is introduced, the θ in the new azimuth evaluation subfunction Nheading(v,ω) is expressed as the angle difference between the current predicted posture and the local sub-target point. At this time, the evaluation function in the algorithm does not take into account the global factor information. In order to better use the global path to guide the navigation of the unmanned boat, the global evaluation component dist is introduced. SG (v, ω) can effectively combine global information and local information to guide the unmanned boat in local obstacle avoidance. At the same time, the steering angular velocity evaluation factor steering(v, ω) is added to the trajectory evaluation function to make the track conform to the kinematic characteristics of the unmanned boat as much as possible, thereby improving the airworthiness. The global evaluation component dist SG The calculation formula of (v,ω) is as follows:

[0098]

[0099] In formula (17), dist ST Indicates the distance between the starting point of the global path and the end of the current predicted trajectory of the unmanned boat, dist TG It indicates the distance between the end of the current predicted trajectory of the unmanned boat and the end point of the global path. When the unmanned boat gradually approaches the end point, the distance between the unmanned boat and the end point is dist TG Gradually decrease, dist SG(v,ω) gradually increases, and the evaluation function is gradually dominated by this term, so that the deviation between the current trajectory and the trajectory obtained by global planning is not too large. At this time, the unmanned boat will select a more suitable trajectory close to the end of the path. The calculation formula of the steering angular velocity evaluation component steering(v,ω) is as follows:

[0100] steering(v,ω)=(|ω max |-|ω·Δt|) / |ω max | (18)

[0101] In formula (18), ω max is the maximum angular velocity of the unmanned boat, and ω is the turning angular velocity at the current moment.

[0102] When avoiding moving obstacle ships, unmanned boats should comply with relevant maritime ship navigation collision avoidance regulations. Since the traditional dynamic window method does not consider the compliance and application of collision avoidance rules, this paper improves the dynamic window method to ensure that the selected velocity vector and the formed collision avoidance path meet the requirements of collision avoidance rules. Article 16 of the International Regulations for Preventing Collisions at Sea (COLREGs) requires that the avoidance action of the way-giving ship should comply with the principles of early, large, wide and clear, that is, the way-giving ship should give way early and significantly to ensure that the two ships pass and give way at a wide distance. Therefore, it is particularly important to fully consider the encounter situation formed by the unmanned boat and other obstacle ships, and then take corresponding avoidance actions. When the unmanned boat meets other ships, by calculating the safe distance R of the encounter situation safe The relative parameter difference between the relative distance D of the two ships in the longitudinal direction determines whether there is a collision risk. When D<R safe When the unmanned boat is about to take collision avoidance action, the collision avoidance rule function rules(v,ω) is introduced into the evaluation function. By calculating the heading angle difference between the two ships, To judge the encounter situation, and then determine the turning direction and turning amplitude ΔC;

[0103] For example, when According to COLREGs, the unmanned boat should pass from the port side of the obstacle ship. At this time, it needs to turn right by ΔC, and the turning amplitude ΔC = ω x Δt, where ω x is the angular velocity of the unmanned boat at time x, Δt is the dynamic window prediction time, when D ≥ R safe, that is, the unmanned boat can resume sailing only after eliminating the risk of collision with other ships. At this time, the rules(v,ω) component in the evaluation function will no longer work. The timing of resuming sailing is also different for different encounter situations. In the case of head-on encounter, when the other ship is located in the transverse position of the unmanned boat, the unmanned boat starts to resume sailing and accelerates to the destination; in the case of overtaking, when the unmanned boat passes the obstacle ship, it immediately accelerates and starts to resume sailing; in the case of crossing encounter, when the other ship is located in the transverse direction of the unmanned boat, the unmanned boat starts to resume sailing. In summary, the final improved trajectory evaluation function is shown in formula (19):

[0104]

[0105] In formula (19), rules(v,ω) is used to calculate the distance for the unmanned boat to take evasive action, as well as to determine the turning direction, turning amplitude and timing of re-entry. Nheading(v,ω) represents the new azimuth evaluation subfunction after the key point is introduced, τ is the speed dynamic evaluation weight coefficient, steering(v,ω) represents the steering angular velocity evaluation subfunction, and dist SG (v,ω) represents the global evaluation subfunction, R safe is the safe distance for encounter situations, D is the relative distance between the two ships in the longitudinal direction, μ, η and ε correspond to steering(v,ω), dist SG The weight coefficients of the three components of (v, ω) and rules(v, ω) can be dynamically adjusted to allow the collision avoidance rules to be better integrated into the dynamic window method, so that the appropriate velocity vector can be flexibly selected according to the actual sea conditions to complete the collision avoidance operation.

[0106] Based on the segmented fusion strategy and the optimization of the trajectory evaluation function, the improved dynamic window method local path planning design steps are as follows:

[0107] The first step is to initialize the grid map and the environmental information.

[0108] The second step uses the known information to obtain a safe and collision-free global path, which consists of a sequence of path points. The key points on the global optimal path are then extracted and tracked in turn as temporary sub-target points of local path planning.

[0109] The third step is to initialize the dynamic window information, which mainly includes the velocity vector of the dynamic window, the update time of the dynamic window, etc.

[0110] The fourth step is to discretely sample v and ω in the speed sampling space, obtain a series of speed pairs (v, ω) by setting the speed resolution and rotation speed resolution, and generate all predicted state trajectories according to the simulation cycle time.

[0111] The fifth step is to determine whether there is a risk of collision between the dynamic and static obstacles and the unmanned boat. If there are no obstacles around the unmanned boat, the speed and position information will continue to be updated along the optimal trajectory. If there is a risk of collision between the unmanned boat and the static obstacle, a safe distance will be maintained to avoid it. If there is a risk of collision between the unmanned boat and the obstacle ship, the current situation needs to be divided and corresponding avoidance actions need to be taken according to the collision avoidance rules.

[0112] The sixth step considers the sub-target point of the current navigation phase, and calculates the scores of all trajectories based on the new trajectory evaluation function G(v,ω) in formula (19). The trajectory with the highest score is the optimal trajectory at the next moment, and the speed and position information are updated. At this time, the unmanned boat gradually approaches the sub-target point.

[0113] The seventh step determines whether the unmanned boat has reached the final destination. If not, it returns to the fourth step to enter the next iteration cycle until the unmanned boat reaches the destination.

[0114] In order to verify the rationality and effectiveness of the improved dynamic window method proposed in this paper in the local path planning of unmanned boats, the simulation experiment was implemented in the Matlab2022a environment.

[0115] The simulation experiment parameters are set as follows: the maximum linear velocity of the unmanned boat is 3m / s, the maximum angular velocity is 0.3491rad / s, and the maximum linear acceleration is 0.2m / s 2 , the maximum angular acceleration is 0.8727rad / s 2 , the linear velocity resolution is 0.01m / s, the angular velocity resolution is 0.0175rad / s, the sampling time is 0.1s, the simulation prediction time is 3s, the radius of the target area is set to 0.2m, and the weight coefficients of the evaluation function are α=0.05, β=0.2, γ=0.2, μ=0.3, η=0.2, ε=1. The experimental verification is divided into two parts. The first part is to simulate the unmanned boat avoidance process under four encounter situations such as two ships meeting, overtaking, and left and right crossing. The second part is the unmanned boat avoidance process in a mixed scene with both dynamic and static obstacles.

[0116] Assuming that the obstacle ship has a constant heading and speed, it can meet, cross right, cross left, and overtake the ship when meeting. At this time, the obstacle ship continues to sail at the current heading and speed and does not take any collision avoidance action. This paper does not consider the influence of wind, waves and currents. The initial dynamic window method and the improved dynamic window method proposed in this paper are used as collision avoidance strategies for comparison and verification. The safety distance R safe Take 3m.

[0117] (1) The initial parameters of the unmanned boat and obstacle boat in the encounter situation are set as follows:

[0118] Table 1 Initial information of unmanned boat and obstacle ship in encounter situation

[0119]

[0120] like Figure 4-5 As shown in the figure, it is a simulation diagram of the avoidance process of two ships in a situation of encountering each other. Figure 4 The unmanned boat took evasive action when the two ships were about 0.6m apart. Figure 5 The unmanned boat took evasive action when the two ships were about 2.5m apart. Due to the high relative speed of the two ships during the encounter, evasive action should be taken as early as possible in principle. Obviously, the latter's evasive timing is more in line with the requirements of the collision avoidance rules. It is not difficult to see from the avoidance range and avoidance direction that the former turned slightly to the left, while the latter turned sharply to the right. By comparison, it can be seen that when the traditional dynamic window method is used, the unmanned boat can avoid the obstacle ship, but the avoidance timing is late, the avoidance range is small, and the left turn is taken, which is completely inconsistent with the collision avoidance rules. The improved dynamic window method has larger avoidance actions and avoidance angles, and is safer, meeting the requirements of COLREGs for early, large, wide, and clear avoidance actions.

[0121] (2) The initial parameters of the unmanned boat and obstacle boat in the overtaking situation are set as follows:

[0122] Table 2 Initial information of unmanned boat and obstacle ship in overtaking situation

[0123]

[0124] like Figure 6-7 As shown in the figure, it is a simulation diagram of the avoidance process of two ships overtaking each other. Figure 6 The unmanned boat took evasive action when the two ships were about 0.6m apart. Figure 7 The unmanned boat in the middle took evasive action when the two ships were about 1.7m apart, so the timing of the latter's avoidance was more in line with the requirements of the collision avoidance rules. From the avoidance range and avoidance direction, it can be seen that the unmanned boats of the two algorithms turned left, but it is obvious that the avoidance range of the latter is larger, and the latter uses the method of accelerating and bypassing in advance to overtake the obstacle ship. Although it has to go a longer distance on the way, it takes less time than the former. Therefore, considering safety and compliance with collision avoidance rules, the improved dynamic window method performs better.

[0125] (3) The initial parameters of the unmanned boat and obstacle boat in the right crossing situation are set as follows:

[0126] Table 3 Initial information of unmanned boat and obstacle ship in right crossing situation

[0127]

[0128] like Figure 8-9The following is a simulation diagram of the avoidance process of two ships in a right-crossing encounter. Figure 8 The unmanned boat took evasive action when the two ships were about 0.6m apart. Fig. 9 The unmanned boat took evasive action when the two ships were about 2.2m apart, so the latter's avoidance timing was earlier. In addition, when facing the oncoming ship from the right, the former accelerated to turn left and passed in front of the obstacle ship. This avoidance method obviously did not comply with the collision avoidance rules and posed a safety hazard. The latter always maintained a safe meeting distance and passed from the stern of the other ship. The avoidance range was also much greater than that of the former. Although the path planned by the traditional dynamic window method is shorter than that by the improved dynamic window method in terms of path length, from the perspective of safety and compliance with COLREGs, the latter is obviously more advantageous.

[0129] (4) The initial parameters of the unmanned boat and obstacle boat in the left crossing situation are set as follows:

[0130] Table 4 Initial information of unmanned boat and obstacle ship in left crossing situation

[0131]

[0132] like Figure 10-11 As shown in the figure, it is a simulation diagram of the avoidance process in the situation of left crossing of two ships. Similar to the right crossing, the avoidance timing of the unmanned boat in the traditional dynamic window method is late, and it turns slightly to the right from the front of the obstacle ship, while the avoidance timing of the unmanned boat in the improved dynamic window method is early, and it turns significantly to the left from the stern of the obstacle ship, so the latter is more in line with the requirements of the collision avoidance rules.

[0133] In order to fully consider the influence of maritime traffic complexity, the avoidance process of the unmanned boat in a mixed scenario with static and dynamic obstacles is simulated to verify the collision avoidance effect of the proposed algorithm in a multi-obstacle situation. The positions of static and unknown obstacles are as follows: Fig.12 As shown in the figure, the initial parameter settings of the unmanned boat and obstacle boat in the mixed scene are as follows:

[0134] Table 5 Initial information of USV and obstacle ship in mixed scenario

[0135]

[0136]

[0137] like Fig.12As shown in the figure, it is a simulation diagram of the avoidance process in a mixed scenario. When the unmanned boat is navigating in a sea area with multiple obstacles, it first crosses the obstacle ship 1 to the right. According to the collision avoidance rules, the unmanned boat should turn sharply to the right and surpass the obstacle from the stern until it is out of danger and moves closer to the global path according to the guidance of the local sub-target point. In the process of resuming navigation, it encounters the obstacle ship 2 again. At this time, the unmanned boat continues to avoid to the right while maintaining a safe distance. Finally, an unknown obstacle suddenly appears on the way to the target point. The unmanned boat accelerates to bypass and completes the avoidance action. The simulation results show that the improved dynamic window method can achieve safe collision avoidance in mixed scenarios while taking into account the collision avoidance requirements of COLREGs.

[0138] Finally, it should be noted that the above is only a preferred embodiment of the present invention and is not intended to limit the present invention. Although the present invention has been described in detail with reference to the aforementioned embodiments, it is still possible for those skilled in the art to modify the technical solutions described in the aforementioned embodiments or to make equivalent substitutions for some of the technical features therein. Any modifications, equivalent substitutions, improvements, etc. made within the spirit and principles of the present invention should be included in the protection scope of the present invention.

Claims

1. A local path planning method for an unmanned surface vehicle based on an improved dynamic window method, characterized in that: The following steps are involved: Step 1: Initialize the environmental information, analyze the local path planning problem in combination with the International Regulations for Avoiding Collisions at Sea, and initialize the grid map matrix based on the known information in the current environment, including the starting point and destination information of the unmanned boat and the dynamic and static obstacle information; Step 2: The dynamic window method obtains the corresponding trajectory within the prediction simulation period according to the motion model parameters of the unmanned boat, and establishes the motion model of the unmanned boat. The velocity space of the unmanned boat is determined according to the established motion model and motion constraints. The position and posture of the unmanned boat in ship kinematics are expressed by vector Indicates that x t and t They represent the coordinates of the unmanned boat at time t in the coordinate system, It represents the heading angle of the unmanned boat at time t. In each sampling period, the motion trajectory of the unmanned boat is approximated, and the motion path in the sampling period is regarded as a straight line. The displacement and angle relationship of the unmanned boat at time t+1 relative to time t in the geodetic coordinate system is: Among them, v t ,ω t are the linear velocity and angular velocity of the unmanned boat at time t, respectively, and Δt is the unit sampling time; Step 3: Discretely sample the linear velocity and angular velocity in the velocity vector space to simulate the motion trajectory of the unmanned boat, and use the evaluation function to evaluate each speed combination; The ideal trajectory speed is selected to control the unmanned boat to perform the moving operation, where the evaluation function G(v,ω) is expressed as: G(v,ω)=σ(α·heading(v,ω)+β·dist(v,ω)+γ·velocity(v,ω)), where heading(v,ω) represents the azimuth evaluation subfunction, dist(v,ω) represents the safety distance evaluation subfunction, velocity(v,ω) represents the speed evaluation subfunction, α is the weight of the target azimuth, β is the weight of the safety distance from the obstacle, γ is the weight of the speed, and σ is the normalization function; Step 4: Optimize and improve the traditional dynamic window algorithm, use segmented fusion strategy to improve the local optimal trajectory, and integrate collision avoidance rules to comprehensively improve the trajectory evaluation function for safe avoidance in different encounter situations and mixed scenarios; Before using the local path planning algorithm for dynamic obstacle avoidance, all nodes on the path are segmented and screened, and the key points on the global path are tracked for movement. The adaptive speed weight coefficient τ is introduced to dynamically adjust the speed evaluation subfunction. The expression of τ is: Where c1 and c2 are parameters between [0,1], dist min is the minimum distance between the ship trajectory and the obstacle, R obs Indicates the distance threshold for conflict determination.

2. The local path planning method for an unmanned surface vehicle based on an improved dynamic window method according to claim 1, characterized in that: In the step 1, the collision avoidance rules are integrated into the collision avoidance method of the unmanned boat, and the common encounter situations are divided into four types: head-on encounter, right crossing, left crossing and overtaking. It is safer for the unmanned boat to perform avoidance actions as the active avoidance party. When the unmanned boat and the obstacle encounter the above four encounter situations, the unmanned boat is a give-way boat, and other obstacle boats are defined as straight-moving boats.

3. The local path planning method for an unmanned surface vehicle based on an improved dynamic window method according to claim 1 is characterized in that: In step 2, by continuously updating v t and ω t , the position and heading angle of the unmanned boat in the entire simulation cycle can be calculated, and the dynamic window method includes the maximum and minimum speed constraints V s , Motor acceleration and deceleration constraints V d and braking distance constraint V SD , define the final feasible velocity space set as V r =V s ∩V d ∩V SD .

4. The method for local path planning of an unmanned surface vehicle based on an improved dynamic window method according to claim 1, characterized in that: In step 4, by introducing the global evaluation component dist SG (v,ω) improves the evaluation function, the global evaluation component dist SG The calculation formula for (v,ω) is:

5. The method for local path planning of an unmanned surface vehicle based on an improved dynamic window method according to claim 4, characterized in that: dist ST Indicates the distance between the starting point of the global path and the end of the current predicted trajectory of the unmanned boat, dist TG Indicates the distance between the end of the current predicted trajectory of the unmanned boat and the end of the global path.

6. The method for local path planning of an unmanned surface vehicle based on an improved dynamic window method according to claim 5, characterized in that: Add the steering angular velocity evaluation factor steering(v,ω) to the evaluation function. The calculation formula of the steering angular velocity evaluation component steering(v,ω) is: steering(v,ω)=(|ω max |-|ω·Δt|) / |ω max |.

7. The method for local path planning of an unmanned surface vehicle based on an improved dynamic window method according to claim 6, characterized in that: ω max is the maximum angular velocity of the unmanned boat, and ω is the turning angular velocity at the current moment.

8. The method for local path planning of an unmanned surface vehicle based on an improved dynamic window method according to claim 1, characterized in that: The collision avoidance rules are integrated into the evaluation function of the dynamic window method to ensure that the selected velocity vector and the formed collision avoidance path meet the requirements of the collision avoidance rules. The improved trajectory evaluation function can be expressed as:

9. The method for local path planning of an unmanned surface vehicle based on an improved dynamic window method according to claim 8, characterized in that: rules(v,ω) is used to calculate the distance for the unmanned boat to take evasive action, as well as to determine the turning direction, turning amplitude and timing of re-entry. Nheading(v,ω) represents the new azimuth evaluation subfunction after the key point is introduced, τ is the speed dynamic evaluation weight coefficient, steering(v,ω) represents the steering angular velocity evaluation subfunction, and dist SG (v,ω) represents the global evaluation subfunction, R safe is the safe distance for encounter situations, D is the relative distance between the two ships in the longitudinal direction, μ, η and ε correspond to steering(v,ω), dist SG The weight coefficients of the three components G(v,ω) and rules(v,ω) can be dynamically adjusted to allow the collision avoidance rules to be integrated into the dynamic window method.

Citation Information

Cited By

  • Multi-unmanned ship gathering path planning method and system

    CN119336035A

  • Ocean robot obstacle avoidance trajectory planning method and system considering flow-induced position deviation

    CN119759027A

  • Intelligent positioning and navigation early warning system and method for ships and boats

    CN120084341A

  • Unmanned aerial vehicle local path planning method based on improved dynamic window method

    CN120370977A

  • Path planning method based on target characteristics

    CN120668139A