Intelligent ship autonomous navigation local route planning method based on fast random extension tree under dynamic obstacle
By combining the fast random expansion tree and speed obstacle method in the local track planning of intelligent ships, dynamic window method and obstacle exploration sector strategy, optimize the track planning of intelligent ships, the safety deficiency and track failure in the existing technology are solved. Optimization issues achieve more efficient and safer navigation.
Patent Information
- Application Number
- CN202411168311.9
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2024-08-23
- Publication Date
- 2024-12-03
AI Technical Summary
The existing intelligent ship local track planning methods have problems such as insufficient safety, unoptimized tracks, no kinematic characteristics and irrelevant static obstacles when dealing with dynamic unknown environments, resulting in long navigation time, large energy consumption and low planning efficiency.
The local track planning method for autonomous navigation of intelligent ships based on fast random expansion trees is adopted, combined with the velocity obstacle method and dynamic window method, and the evaluation function of the motion characteristics of intelligent ships is established, taking into account the impact of bow shaking angular velocity and side slip angle, and exploring sectors through obstacles Strategy optimizes tracks to ensure safety and efficiency.
It improves the economic and real-time nature of the track, enhances the dynamic collision avoidance and adjustment ability, reduces the impact of irrelevant static hindrance on the planning, and improves the efficiency and accuracy of local trajectory planning.
Smart Images

Figure CN119065392A_ABST
Abstract
Description
Technical Field
[0001] The present invention belongs to the technical field of local track planning for autonomous navigation of intelligent ships, and in particular, relates to a local track planning method for autonomous navigation of intelligent ships based on a fast random expansion tree under dynamic obstacles. Background Art
[0002] In the actual navigation of smart ships, they may encounter single or even multiple unknown obstacles. The global planning method that relies on prior map information is difficult to handle the problem of ship safety planning in dynamic unknown environments. Local planning is based on real-time perceived online information, so it can be used to solve the problem of safe trajectory generation in unknown dynamic environments.
[0003] As a hot research direction in the current field of marine engineering technology, local track planning of intelligent ships involves multiple technical aspects such as ship autonomous navigation, track optimization, and obstacle avoidance decision-making. The local planning problem of intelligent ships has attracted extensive research. The existing method proposes a fusion method of sampling principle and speed obstacle method to achieve local collision avoidance of ships, but this method does not consider the track direction after collision avoidance. In addition, the existing technology uses the advantages of strong real-time performance and complete probability of the fast random expansion tree method to propose an improved bidirectional fast random expansion tree method to complete the local planning of ships. However, this method does not consider the actual ship model and therefore has the problem of insufficient safety. Moreover, the existing method fully combines the ability of the fast random expansion tree method to replan the map and the speed obstacle method to construct the speed space, and proposes a local track planning method based on the speed obstacle method and the fast random expansion tree method, which improves the real-time and safety of ship obstacle avoidance. Although scholars have proposed a variety of local track planning methods for intelligent ships, the existing methods still have the following problems:
[0004] First, in the existing methods, although feasible safe speed vectors can be solved, the optimal solution of the speed vector cannot be obtained. This means that although the ship can sail safely, its route may not be the most economical or efficient, which leads to problems such as long sailing time and high energy consumption.
[0005] Second, the existing methods do not consider the kinematic and dynamic characteristics of intelligent ships in the intelligent ship planning process, especially ignoring the influence of drift angle, which will cause deviations in the actual tracking and control process of intelligent ships, thereby affecting the accuracy and reliability of navigation and is not conducive to actual tracking control.
[0006] Third, the existing methods do not consider irrelevant static obstacles in the environment during the intelligent ship planning process. Although these obstacles do not pose a direct collision risk to the ship's navigation, if they are not considered in the trajectory planning, the planning results may not be optimal, reducing the efficiency of local trajectory planning. Summary of the invention
[0007] In order to solve the local track planning problem of intelligent ships with dynamic obstacles, the technical solution adopted by the present invention is: a local track planning method for autonomous navigation of intelligent ships based on fast random expansion tree under dynamic obstacles includes the following steps:
[0008] Establish underactuated intelligent ship model and two-dimensional motion model;
[0009] The speed barrier method is introduced within the framework of the fast randomized expanding tree algorithm to solve the speed vector that satisfies the safety constraints of the underactuated intelligent ship.
[0010] Based on the speed constraints of intelligent ships, the dynamic window method is combined with the speed barrier method to establish the evaluation function of the motion characteristics of intelligent ships. The optimal speed combination of intelligent ships is determined by considering the influence of the bow angular velocity and the sideslip angle, and the local optimal track that meets the motion characteristics of the ship is selected.
[0011] Based on the obstacle exploration sector strategy, the process control of intelligent ships passing through dense static obstacle areas is addressed.
[0012] Further: the process of introducing the speed barrier method within the framework of the fast random expansion tree algorithm to solve the speed vector that satisfies the safety constraints of the under-actuated intelligent ship is as follows:
[0013] In the earth coordinate system, define U ob is the speed of the dynamic obstacle, R is the expansion circle radius of the dynamic obstacle, U is the speed of the smart ship, U′ is the relative speed between the dynamic obstacle and the smart ship, line segment D0 is the line connecting the center of the smart ship and the center of the dynamic obstacle, λ1 and λ2 are two tangent lines of the left and right boundaries of the obstacle after expansion, with the center of the smart ship as the vertex;
[0014] Therefore, in order to avoid collision between smart ships and dynamic obstacles, the following conditions must be met:
[0015]
[0016] in, is the angle between U′ and D0, θ half is the angle between the tangent lines λ1 and λ2 and D0, and the solution is The corresponding ship speed is the safe speed vector;
[0017] Definition L vo The intelligent ship starts from the current position and extends the distance along the safe speed vector direction. The ship starts from the current position and extends along L vo Get a new position and record this position as the initial node x of the fast random expansion tree algorithminit , and then add this initial node to the tree to start path expansion based on the fast randomized expanding tree algorithm.
[0018] Further: the intelligent ship starts from the current position and extends the distance L in the direction of the safety speed vector vo The calculation formula is as follows:
[0019] L vo =D0+R (5)
[0020] Where: D0 is the line between the center of the intelligent ship and the center of the dynamic obstacle, R is the expansion circle radius of the dynamic obstacle;
[0021] Consider optimizing the expansion circle radius R of the dynamic obstruction and design it as follows:
[0022]
[0023] Among them, r is the radius of the obstacle circle when the dynamic window method is used to process static obstacles, and d1 is a constant.
[0024] Furthermore, the speed constraints of the intelligent ship include the maximum and minimum speed range constraints that can be provided by the intelligent ship power system, the speed range V that can be actually achieved by the intelligent ship, and the speed interval V j and the constraint that the smart ship has enough time to reduce the speed and angular velocity to zero before encountering the obstacle.
[0025] Furthermore: The maximum and minimum speed range constraints that the intelligent ship power system can provide are as follows:
[0026] V i ={(U i , r Wi )|U i ∈[U min , U max ],r Wi ∈[r Wmin , r Wmax ]} (7)
[0027] Among them, U min is the minimum speed of the smart ship; U max is the maximum speed of the smart ship; r Wmin is the minimum angular velocity of the smart ship; r Wmax V is the maximum angular velocity of the smart ship; i is the maximum and minimum speed range constraints that the intelligent ship power system can provide, U i and r Wi are the velocity and angular velocity that satisfy the constraints respectively;
[0028] The speed interval V of the speed range that the smart ship can actually achieve j The constraints are as follows:
[0029]
[0030] Among them, U j and r Wj They are the speed intervals V that meet the actual achievable speed range of intelligent ships. j The speed and angular velocity of the constraints; U c is the current speed of the smart ship; is the maximum acceleration of the smart ship; is the maximum deceleration of the smart ship; r Wc is the current angular velocity of the smart ship; The minimum angular acceleration of the smart ship; is the maximum angular deceleration of the smart ship;
[0031] The constraints that the intelligent ship has enough time to reduce the speed and angular velocity to zero before encountering an obstacle are as follows:
[0032]
[0033] Among them, dist is the shortest distance between the corresponding simulated trajectory and the obstacle at the current speed of the intelligent ship;
[0034] U k and r Wk are the speed and angular velocity respectively satisfying the constraint that the intelligent ship has enough time to reduce the speed and angular velocity to zero before encountering an obstacle; is the maximum deceleration of the smart ship; It is the minimum angular acceleration of the smart ship.
[0035] Further: the evaluation function form of the motion characteristics of the intelligent ship is:
[0036]
[0037] Where α, β, γ, κ and λ are the weight coefficients of each evaluation function, palstance(U, r W ) is the evaluation function of the yaw angular velocity; driftangle(U, r W ) is the evaluation function of the sideslip angle σ, heading? (U, r W ) is the deflection angle evaluation function; distance? (U, r W ) is the distance evaluation function, which indicates the shortest distance between the trajectory corresponding to the current sampling speed and the dynamic obstacle; velocity(U, rW ) is the speed evaluation function. On the basis of meeting the safety obstacle avoidance requirements, this evaluation function can screen out the path with the highest navigation speed, aiming to reach the target point in the shortest time;
[0038] Among them, the bow angular velocity evaluation function palstance(U, r W ) is designed as follows:
[0039] palstance(U,r W )=(r Wmax -|r W |) / r Wmax (14)
[0040] In the formula, r W is the current bow angular velocity of the smart ship; r Wmax is the maximum bow angular velocity of the smart ship;
[0041] Side slip angle evaluation function driftangle(U, r W ) is designed as follows:
[0042] driftangle(U,r W )=π-|σ| (15)
[0043] Further: the obstacle exploration sector strategy is as follows:
[0044] Consider the following scenario: define the predicted position of the smart ship after T0 as P, the predicted speed of the smart ship as U0, O1, O2 and O3 as the distances between the obstacles around the smart ship and the smart ship, d1, d2 and d3 respectively, d1<d2<d3, according to formula (8), at this time dist=d1, if the smart ship has bypassed obstacle O1 and the distance between it and the obstacle is increasing, in order to optimize the route, obstacle O1 should no longer be considered;
[0045] For the above scenario, an obstacle detection area is established, with P as the center of the sector and the angle of the sector area is Establish a fan-shaped area, and the smart ship divides the fan-shaped area into two equal parts along its speed direction. The value is
[0046] If there is no intersection with the obstacle expansion circle in this fan-shaped area, the smart ship will no longer consider the obstacle and does not need to avoid it;
[0047] Therefore, under this obstacle detection sector strategy, the shortest distance between the obstacle and the smart ship is d2.
[0048] The invention provides a local track planning method for autonomous navigation of intelligent ships based on fast random expansion tree under dynamic obstacles. The fast random expansion tree method is combined with a speed obstacle method and a dynamic window method to propose a local track planning method for autonomous navigation of intelligent ships based on fast random expansion tree under dynamic obstacles.
[0049] Firstly, the speed barrier method is introduced into the framework of the fast randomized expanding tree algorithm to solve the speed vector that satisfies the safety constraints. The fast randomized expanding tree algorithm is used to enhance the map update to ensure that the smart ship can determine the speed set for real-time safe collision avoidance. An adjustment strategy is designed for the expansion radius of the speed barrier method to avoid the inability of the smart ship to start.
[0050] The dynamic window method is further used to integrate the speed barrier method to establish an improved evaluation function and find the optimal speed combination. In this process, the influence of the bow angular velocity and the sideslip angle is considered to select the local optimal track that meets the ship's motion characteristics.
[0051] An obstacle exploration sector strategy is proposed for static obstacles, which reduces the impact of irrelevant static obstacles on the planning process.
[0052] The present invention improves the track length and the real-time performance of the method and has a stronger dynamic collision avoidance adjustment capability.
[0053] Compared with the prior art, the present invention has the following advantages:
[0054] First, compared with the existing fast random expanding tree local planning method based on the speed barrier method, the present invention introduces the dynamic window method, takes the ship state as an indicator to construct an evaluation function, searches for the optimal solution in the feasible safe speed set, and realizes the optimization of the safe trajectory.
[0055] Second, compared with the existing planning method based on fixed expansion radius, the present invention adopts a dynamic radius expansion strategy, dynamically adjusting the expansion range of the obstacle according to the relative distance between the dynamic obstacle and the smart ship, avoiding the starting dilemma of the smart ship under a fixed radius.
[0056] Third, compared with the existing local planning methods that do not consider the dynamic characteristics of the ship, the present invention not only introduces the ship speed, acceleration, and angular velocity into the construction of the evaluation function, but also considers the sideslip motion of the ship in the planning process, which is beneficial to the tracking control of the intelligent ship.
[0057] Fourth, compared with the existing local planning method that does not consider irrelevant obstacles, the present invention eliminates obstacles that do not affect the subsequent navigation of the smart ship by proposing an obstacle exploration sector strategy, thereby improving the collision avoidance capability of the smart ship in a dense static obstacle area. BRIEF DESCRIPTION OF THE DRAWINGS
[0058] In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings required for use in the embodiments or the description of the prior art will be briefly introduced below. Obviously, the drawings described below are some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying creative labor.
[0059] Figure 1 is a flowchart of the method;
[0060] Figure 2 It is a schematic diagram of local planning of smart ships;
[0061] Figure 3 It is the principle diagram of the speed barrier method;
[0062] Figure 4 is a schematic diagram of the obstacle exploration sector;
[0063] Figure 5 A safe trajectory diagram planned for smart ships to avoid dynamic obstacles 1.
[0064] Figure 6 A safe trajectory diagram planned for smart ships to avoid dynamic obstacles 2.
[0065] Figure 7 A safe trajectory diagram planned for smart ships to avoid dynamic obstacles 3.
[0066] Figure 8 A safe trajectory diagram planned for smart ships to avoid dynamic obstacles 4.
[0067] Fig. 9 Provide a safe trajectory map for smart ships to reach their destination. DETAILED DESCRIPTION
[0068] It should be noted that, in the absence of conflict, the embodiments of the present invention and the features in the embodiments may be combined with each other. The present invention will be described in detail below with reference to the accompanying drawings and in combination with the embodiments.
[0069] In order to make the purpose, technical solutions and advantages of the embodiments of the present invention clearer, the technical solutions in the embodiments of the present invention will be clearly and completely described below in conjunction with the drawings 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. The following description of at least one exemplary embodiment is actually only illustrative and is by no means intended to limit the present invention and its application or use. 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.
[0070] Figure 1is a flowchart of the method;
[0071] Figure 2 This is a schematic diagram of the local planning of a smart ship.
[0072] A local track planning method for autonomous navigation of an intelligent ship based on a fast random expansion tree under dynamic obstacles comprises the following steps:
[0073] S1: Establish an underactuated intelligent ship model and a two-dimensional motion model;
[0074] S2: Introducing the speed barrier method within the framework of the fast randomized expanding tree algorithm to solve the speed vector that satisfies the safety constraints of the underactuated intelligent ship;
[0075] S3: Based on the speed constraints of the intelligent ship, the dynamic window method is integrated with the speed barrier method to establish the evaluation function of the motion characteristics of the intelligent ship. The optimal speed combination of the intelligent ship is determined by considering the influence of the bow angular velocity and the sideslip angle, and the local optimal track that meets the motion characteristics of the ship is selected;
[0076] S4: Process control of intelligent ships passing through dense static obstacle areas based on obstacle exploration sector strategy.
[0077] Steps S1 / S2 / S3 / S4 are executed sequentially;
[0078] Furthermore, the underactuated intelligent ship model and the two-dimensional motion model of local planning are established as follows:
[0079] Define the total speed of the smart ship as The heading angle is ψ w =ψ+β, where β=arctan(v / u), then the underactuated intelligent ship model is as follows:
[0080]
[0081] in, is the angular velocity of the smart ship;
[0082] a U =u[f u (·)+τ du +τ u ] / (Um u )+v[f v (·)+τ dv ] / (Um v ) is the acceleration of the smart ship.
[0083] Since the driving ability of smart ships is limited during actual navigation, in order to fully consider these physical constraints, the constraints on the speed and acceleration of smart ships are defined as:
[0084]
[0085] Among them, U max =10m / s, r min =-0.4rad / s 2 , r max =0.4rad / s 2 , a min =-0.5m / s 2 , a max =0.5m / s 2 .
[0086] Define the coordinates of the ship's position at time t in the earth coordinate system as (x(t), y(t)), U(t) and ψ w (t) are the speed and bow angular velocity of the smart ship at time t respectively.
[0087] If a shorter sampling period is selected, the navigation track of the ship in each sampling period can be replaced by a straight line, and the position of the ship at time t+1 can be expressed as follows:
[0088]
[0089] From the above formula, it can be deduced that the ship moves linearly along the heading angle relative to the earth coordinate system U(t)Δt, and the heading angle rotates r W (t)Δt.
[0090] Figure 3 It is the principle diagram of the speed barrier method;
[0091] Furthermore, the process of introducing the speed barrier method into the framework of the random expansion tree algorithm to solve the speed vector that satisfies the safety constraints of the underactuated intelligent ship is as follows:
[0092] In the earth coordinate system, define U ob is the speed of the dynamic obstacle, R is the expansion circle radius of the dynamic obstacle, U is the speed of the smart ship, U′ is the relative speed between the dynamic obstacle and the smart ship, line segment D0 is the line connecting the center of the smart ship and the center of the dynamic obstacle, λ1 and λ2 are two tangent lines of the left and right boundaries of the obstacle after expansion, with the center of the smart ship as the vertex;
[0093] Therefore, in order to avoid collision between smart ships and dynamic obstacles, the following conditions must be met:
[0094]
[0095] in, is the angle between U′ and D0, θ half is the angle between the tangent lines λ1 and λ2 and D0, and the solution is The corresponding ship speed is the safe speed vector;
[0096] After solving the safe speed vector, define L vo The intelligent ship starts from the current position and extends the distance along the safe speed vector direction. The ship starts from the current position and extends along L vo Get a new position and record this position as the initial node x of the fast random expansion tree algorithm init , and then add this initial node to the tree to start path expansion based on the fast randomized expanding tree algorithm.
[0097] In order to reduce the impact of the randomness of the fast random expansion tree and reduce the possibility of excessive avoidance, the intelligent ship starts from its current position and expands the distance L in the direction of the safety speed vector vo The calculation formula is as follows:
[0098] L vo =D0+R (5)
[0099] Where: D0 is the line between the center of the intelligent ship and the center of the dynamic obstacle, R is the expansion circle radius of the dynamic obstacle;
[0100] Consider optimizing the expansion circle radius R of the dynamic obstruction and design it as follows:
[0101]
[0102] Among them, r is the radius of the obstacle circle when the dynamic window method is used to process static obstacles, and d1 is a constant.
[0103] Before designing the evaluation indicators for intelligent ship trajectory planning, the constraints on the speed of the intelligent ship should be analyzed first.
[0104] Specifically, when solving the speed, the dynamic window method needs to consider the constraints on the ship's speed. The constraints on the ship's speed include the maximum feasible interval, which is the maximum and minimum speed ranges that the intelligent ship power system can provide, and the speed range V that the intelligent ship can actually achieve. j and the constraint that the smart ship has enough time to reduce the speed and angular velocity to zero before encountering the obstacle.
[0105] The maximum and minimum speed range constraints that the intelligent ship power system can provide are as follows:
[0106] V i ={(U i , r Wi )|U i ∈[U min , Umax ],r Wi ∈[r Wmin , r Wmax ]} (7)
[0107] Among them, V i is the maximum and minimum speed range constraints that the intelligent ship power system can provide, U i and r wi are the speed and angular velocity that satisfy the constraints respectively; U min is the minimum speed of the smart ship; U max is the maximum speed of the smart ship; r Wmin is the minimum angular velocity of the smart ship; r Wmax is the maximum angular velocity of the smart ship;
[0108] The speed interval V of the speed range that the smart ship can actually achieve j The constraints are as follows:
[0109]
[0110] Among them, U j and r Wj They are the speed intervals V that meet the actual achievable speed range of intelligent ships. j The speed and angular velocity of the constraints; U c is the current speed of the smart ship; is the maximum acceleration of the smart ship; is the maximum deceleration of the smart ship; r Wc is the current angular velocity of the smart ship; The minimum angular acceleration of the smart ship; is the maximum angular deceleration of the smart ship;
[0111] In order to ensure that the dynamic window method can perform dynamic real-time collision avoidance when performing local planning, it should be ensured that the intelligent ship has enough time to reduce the speed and angular velocity to zero before encountering the obstacle. Therefore, the constraint conditions for the intelligent ship to have enough time to reduce the speed and angular velocity to zero before encountering the obstacle are as follows:
[0112]
[0113] Wherein, dist is the shortest distance between the corresponding simulated trajectory and the obstacle at the current speed of the intelligent ship.
[0114] U k and r Wk are the speed and angular velocity respectively satisfying the constraint that the intelligent ship has enough time to reduce the speed and angular velocity to zero before encountering an obstacle; is the maximum deceleration of the smart ship; Minimum angular acceleration for smart ships
[0115] According to the above three speed constraints, the dynamic window method sampling speed space is:
[0116] V r =V i ∩V j ∩V k (10)
[0117] Based on this sampling space, we can get the range of speed and angular velocity. By combining different speed and angular velocity samples and combining them with the forward simulation time, we can derive the predicted navigation trajectory in the next cycle, and then use the evaluation function to comprehensively evaluate each trajectory. In summary, there are constraints first, then the feasible speed and angular velocity range is obtained, and then the evaluation function is designed to evaluate the trajectory. This is a complete process, which uses a combination of the dynamic window method and the speed barrier method.
[0118] The motion of the smart ship in each sampling period is approximated as linear motion. Based on different speed and angular velocity sampling combinations and combined with the forward simulation time, the predicted navigation trajectory in the next period can be derived respectively, and then the evaluation function is used to comprehensively evaluate each trajectory. The basic form of the evaluation function is as follows:
[0119]
[0120] Among them, heading(U, r W ) is the deflection angle evaluation function; distance(U, r W ) is the distance evaluation function; velocity(U, r W ) is the speed evaluation function; α, β and γ are the weight coefficients of each evaluation function.
[0121] Usually, we want to use the larger the evaluation function, the better it is, so we set heading(U, r W ) is designed as follows:
[0122] heading(U,r W )=π-Δζ (12)
[0123] Among them, Δζ is the angular error between the position direction of the trajectory end point generated at the current sampling speed within the sampling period and the line connecting the target point.
[0124] The sideslip angle σ is the deviation between the speed direction and the heading angle of the smart ship, which is mainly affected by multiple factors such as the lateral coupling speed, ocean currents, and external environmental interference when the ship turns. This deviation will not only interfere with the maneuverability of the ship, but may also lead to energy waste. In order to ensure the stability and efficiency of the smart ship's navigation, the sideslip angle σ must be reasonably restricted. At the same time, given that the research object considered in this article is an under-actuated smart ship, its steering movement mainly depends on the adjustment of the rudder angle. However, there is a limit value for the rudder angle, and time delays may occur during the steering process. In contrast, the use of a smaller bow angular velocity is not only easier to achieve, but also helps to improve the navigation performance of smart ships and further reduce energy consumption. When constructing the evaluation function of the dynamic window method, these motion characteristics of smart ships must be fully considered. Therefore, the evaluation function combined with the motion characteristics of smart ships is in the form of:
[0125]
[0126] Where α, β, γ, κ and λ are the weight coefficients of each evaluation function, palstance(U, r W ) is the evaluation function of the yaw angular velocity; driftangle(U, r W ) is the evaluation function of the sideslip angle σ, heading(U, r W ) is the deflection angle evaluation function; distance(U, r W ) is the distance evaluation function, which indicates the shortest distance between the trajectory corresponding to the current sampling speed and the dynamic obstacle; velocity(U, r W ) is the speed evaluation function. On the basis of meeting the safety obstacle avoidance requirements, this evaluation function can screen out the path with the highest navigation speed, aiming to reach the target point in the shortest time;
[0127] Among them, the bow angular velocity evaluation function palstance(U, r W ) is designed as follows:
[0128] palstance(U,r W )=(r Wmax -|r W |) / r Wmax (14)
[0129] In the formula, r W is the current bow angular velocity of the smart ship; r Wmax is the maximum bow angular velocity of the smart ship;
[0130] Side slip angle evaluation function driftangle(U, r W ) is designed as follows:
[0131] driftangle(U,r W )=π-|σ| (15)
[0132] Figure 4 is a schematic diagram of the obstacle exploration sector;
[0133] In view of the sudden static obstacles during local planning, a strategy of establishing obstacle detection sectors is considered to deal with the intelligent ship passing through dense static obstacle areas.
[0134] Define the predicted position of the smart ship after T0 as P, the predicted speed of the smart ship as U0, O1, O2 and O3 as obstacles around the smart ship, and the distances between the obstacles and the smart ship as d1, d2 and d3, respectively. d1<d2<d3. According to formula (8), at this time, dist=d1. If the ship has bypassed obstacle O1 and the distance from it is increasing, in order to optimize the route, obstacle O1 should no longer be considered.
[0135] For the above scenario, an obstacle detection area is established, with P as the center of the sector and the angle of the sector area is A sector area is established, and the smart ship divides the sector area equally along its speed direction. In this example, The value is If there is no intersection with the obstacle expansion circle in this sector area, the smart ship will no longer consider the obstacle and does not need to avoid it. Therefore, under this obstacle detection sector strategy, the closest distance between the obstacle and the smart ship is d2.
[0136] The present invention will be further described below in conjunction with the accompanying drawings.
[0137] Figure 5 The ship encounters dynamic obstacle 1 and turns left to avoid it while avoiding static obstacles. After the obstacle avoidance is completed, the ship continues to plan the track towards the destination.
[0138] Figure 6 The ship encounters dynamic obstacle 2 and turns left to avoid it while avoiding static obstacles. After the obstacle avoidance is completed, the ship continues to sail towards the destination.
[0139] Figure 7 The ship encounters dynamic obstacle 3 and turns left to avoid it, while avoiding static obstacles at the same time. After the obstacle avoidance is completed, the ship continues to plan the track towards the end point.
[0140] Figure 8 The ship encounters dynamic obstacle 4 and turns left to avoid it, while avoiding static obstacles at the same time. After the obstacle avoidance is completed, the ship continues to plan the track towards the end point.
[0141] Fig. 9This is an overall display of the planned track, and it can be seen that the ship arrived at the destination safely.
[0142] It can be seen from the above simulation result diagram that the present invention can enable the intelligent ship to perform real-time replanning, which verifies the effectiveness of the local track planning method proposed in the present invention.
[0143] The present invention is not limited to this embodiment, and any equivalent concepts or changes within the technical scope disclosed by the present invention are included in the protection scope of the present invention.
[0144] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention, rather than to limit it. Although the present invention has been described in detail with reference to the aforementioned embodiments, those skilled in the art should understand that they can still modify the technical solutions described in the aforementioned embodiments, or replace some or all of the technical features therein with equivalents. However, these modifications or replacements do not cause the essence of the corresponding technical solutions to deviate from the scope of the technical solutions of the embodiments of the present invention.
Claims
1. A local track planning method for autonomous navigation of intelligent ships based on fast random expansion tree under dynamic obstacles. Features: The following steps are involved: Establish underactuated intelligent ship model and two-dimensional motion model; The speed barrier method is introduced within the framework of the fast randomized expanding tree algorithm to solve the speed vector that satisfies the safety constraints of the underactuated intelligent ship. Based on the speed constraints of intelligent ships, the dynamic window method is combined with the speed barrier method to establish the evaluation function of the motion characteristics of intelligent ships. The optimal speed combination of intelligent ships is determined by considering the influence of the bow angular velocity and the sideslip angle, and the local optimal track that meets the motion characteristics of the ship is selected. Based on the obstacle exploration sector strategy, the process control of intelligent ships passing through dense static obstacle areas is addressed.
2. According to claim 1, a local track planning method for autonomous navigation of intelligent ships based on fast random expansion tree under dynamic obstacles, Features: The process of introducing the speed barrier method within the framework of the fast random expanding tree algorithm to solve the speed vector that satisfies the safety constraints of the underactuated intelligent ship is as follows: In the earth coordinate system, define U ob is the speed of the dynamic obstacle, R is the expansion circle radius of the dynamic obstacle, U is the speed of the smart ship, U′ is the relative speed between the dynamic obstacle and the smart ship, and line segment D 0 Connect the intelligent ship center and the dynamic obstacle center, 1 and λ 2 The two tangent lines of the left and right boundaries of the obstruction after expansion, with the center of the smart ship as the vertex; Therefore, in order to avoid collision between smart ships and dynamic obstacles, the following conditions must be met: in, U′ and D 0 The angle between half is the tangent line λ 1 and λ 2 With D 0 The angle between The corresponding ship speed is the safe speed vector; Definition L vo The intelligent ship starts from the current position and extends the distance along the safe speed vector direction. The ship starts from the current position and extends along L vo Get a new position and record this position as the initial node x of the fast random expansion tree algorithm init , and then add this initial node to the tree to start path expansion based on the fast randomized expanding tree algorithm.
3. According to claim 2, a local track planning method for autonomous navigation of intelligent ships based on fast random expansion tree under dynamic obstacles, Features: The intelligent ship starts from its current position and expands the distance L in the direction of the safety speed vector vo The calculation formula is as follows: L vo =D 0 +R (5) Where: D 0 is the line connecting the center of the intelligent ship and the center of the dynamic obstacle, and R is the radius of the expansion circle of the dynamic obstacle; Consider optimizing the expansion circle radius R of the dynamic obstruction and design it as follows: Among them, r is the radius of the obstacle circle when the dynamic window method is used to process static obstacles, and d 1 is a constant.
4. According to claim 2, a local track planning method for autonomous navigation of intelligent ships based on fast random expansion tree under dynamic obstacles, Features: The speed constraints of the intelligent ship include the maximum and minimum speed range constraints that can be provided by the intelligent ship power system, the speed range V that can be actually achieved by the intelligent ship, and the speed interval V j and the constraint that the smart ship has enough time to reduce the speed and angular velocity to zero before encountering the obstacle.
5. According to claim 4, a local track planning method for autonomous navigation of intelligent ships based on fast random expansion tree under dynamic obstacles, Features: The maximum and minimum speed range constraints that the intelligent ship power system can provide are as follows: V i ={(U i ,r Wi )∣U i ∈[U min ,U max ],r Wi ∈[r Wmin ,r Wmax ]} (7) Among them, U min is the minimum speed of the smart ship; U max is the maximum speed of the smart ship; r Wmin is the minimum angular velocity of the smart ship; r Wmax V is the maximum angular velocity of the smart ship; i is the maximum and minimum speed range constraints that the intelligent ship power system can provide, U i and r Wi are the velocity and angular velocity that satisfy the constraints respectively; The speed interval V of the speed range that the smart ship can actually achieve j The constraints are as follows: Among them, U j and r Wj They are the speed intervals V that meet the actual achievable speed range of intelligent ships. j The speed and angular velocity of the constraints; U c is the current speed of the smart ship; is the maximum acceleration of the smart ship; is the maximum deceleration of the smart ship; r Wc is the current angular velocity of the smart ship; It is the minimum angular acceleration of smart ships; is the maximum angular deceleration of the smart ship; The constraints that the intelligent ship has enough time to reduce the speed and angular velocity to zero before encountering an obstacle are as follows: Among them, dist is the shortest distance between the corresponding simulated trajectory and the obstacle at the current speed of the intelligent ship; U k and r Wk are the speed and angular velocity respectively satisfying the constraint that the intelligent ship has enough time to reduce the speed and angular velocity to zero before encountering an obstacle; is the maximum deceleration of the smart ship; It is the minimum angular acceleration of the smart ship.
6. According to claim 1, a local track planning method for autonomous navigation of intelligent ships based on fast random expansion tree under dynamic obstacles, Features: The evaluation function form of the intelligent ship motion characteristics is: Where α, β, γ, k and λ are the weight coefficients of each evaluation function, palstance(U, r W ) is the evaluation function of the yaw angular velocity; driftangle(U, r W ) is the evaluation function of the sideslip angle σ, heading(U, r W ) is the deflection angle evaluation function; distance(U, r W ) is the distance evaluation function, which indicates the shortest distance between the trajectory corresponding to the current sampling speed and the dynamic obstacle; velocity(U, r W ) is the speed evaluation function. On the basis of meeting the safety obstacle avoidance requirements, this evaluation function can screen out the path with the highest navigation speed, aiming to reach the target point in the shortest time; Among them, the bow angular velocity evaluation function palstance(U, r W ) is designed as follows: palstance(U,r W )=(r Wmax -|r W |) / r Wmax (14) In the formula, r W is the current bow angular velocity of the smart ship; r Wmax is the maximum bow angular velocity of the smart ship; Side slip angle evaluation function driftangle(U, r W ) is designed as follows: driftangle(U,r W )=π-|σ| (15).
7. According to claim 1, a local track planning method for autonomous navigation of intelligent ships based on fast random expansion tree under dynamic obstacles, Features: The obstacle exploration sector strategy is as follows: Consider the following scenario: Define the T 0 The predicted position of the smart ship is P, and the predicted speed of the smart ship is U 0 , O 1 , O 2 and O 3 The distances between the obstacles around the smart ship and the smart ship are d 1 d 2 and d 3 , d 1 <d 2 <d 3 According to formula (8), at this time dist = d 1 , if the smart ship has bypassed the obstacle O 1 And the distance between them is increasing. To optimize the route, the obstacle O should no longer be considered. 1 ; For the above scenario, an obstacle detection area is established, with P as the center of the sector and the angle of the sector area is Establish a fan-shaped area, and the smart ship divides the fan-shaped area into two equal parts along its speed direction. The value is If there is no intersection with the obstacle expansion circle in this fan-shaped area, the smart ship will no longer consider the obstacle and does not need to avoid it; Therefore, under this obstacle detection sector strategy, the shortest distance between the obstacle and the smart ship is d 2 .
Citation Information
Cited By
Ocean robot obstacle avoidance trajectory planning method and system considering flow-induced position deviation
CN119759027A
Ship control method, device, equipment, medium and product
CN119828708A
