Unmanned ship improved dynamic window local path planning method, program and equipment for autonomous recovery of unmanned marine vehicle, and storage medium

Abstract

According to the method, a double-dynamic window prediction tracking mechanism is introduced, the trajectory of the unmanned ship is simulated, the future trajectory of the unmanned marine vehicle is predicted in parallel, the expected linear velocity and the expected angular velocity of the unmanned ship are calculated by comprehensively considering the simulated trajectories of the unmanned ship and the unmanned ship, unnecessary detour is effectively reduced, and the efficiency of recovery operation is improved. According to the method, the navigational speed dynamic constraint algorithm is fused into the unmanned ship trajectory simulation process, and the linear speed at the simulated trajectory point is dynamically adjusted according to the relative position of the unmanned ship and the unmanned marine vehicle, so that the predicted trajectory of the unmanned ship is closer to a real autonomous recovery task scene. According to the invention, the heading guidance algorithm for autonomous recovery and the speed dynamic constraint algorithm are fused into the dynamic window evaluation algorithm, and each trajectory point of the unmanned ship simulation trajectory is evaluated, so that the whole trajectory is ensured to be capable of fitting the heading guidance algorithm and the speed dynamic constraint algorithm. The problem that a traditional dynamic window method is incompatible with a recovery guidance algorithm is solved.

Description

Technical Field

[0001] The present invention belongs to the technical field of local path planning for unmanned boats, and in particular relates to an improved dynamic window local path planning method, program, device and storage medium for an unmanned boat for autonomous recovery of unmanned ocean vehicles. Background Art

[0002] The technology for combined operations between unmanned submersibles (USVs) and unmanned marine vehicles (UMVs) continues to advance, and autonomous recovery of UMVs by USVs has become a key issue. Autonomous UMV recovery technology can improve efficiency, reduce costs, and increase safety. During autonomous UMV recovery missions, the UMV must maintain linear motion to maintain a stable attitude. Accordingly, the USV's guidance system must possess high precision and flexibility to guide the USV toward the dynamically moving UMV in the appropriate attitude, ensuring synchronization of heading and speed during the approach process, thereby improving the recovery success rate.

[0003] Currently, a variety of guidance algorithms are available for autonomous recovery. When the USV is behind the UMV, pure pursuit guidance (PP) and line-of-sight guidance (LOS) can accomplish this task. When the USV is in front of the UMV, more flexible guidance algorithms such as fuzzy guidance and layered guidance can be used. However, during UMV operations, obstacles such as offshore platforms and buoys are often present. Therefore, obstacle avoidance needs to be considered during the USV's autonomous recovery of the UMV. Guidance algorithms are often combined with local path planning strategies to achieve obstacle avoidance. Among these, the dynamic window method demonstrates significant advantages in real-time path planning and obstacle avoidance for robots, as it comprehensively considers the robot's kinematic and dynamic constraints.

[0004] The patent application, CN202310825434.4, published on October 27, 2023, and titled "A Path Planning Method for Unmanned Watercraft Based on Improved A* and DWA Fusion," provides a path planning method for an unmanned watercraft. The method includes the following steps: initializing parameters and a grid map, setting the starting and target points for path planning; performing global path planning within the initialized map using an improved A* algorithm to form a preliminary global path; processing nodes in the preliminary global path using the quadratic interpolation method to generate a globally optimized path; obtaining key turning points in the globally optimized path and using them as local target points for local path planning using the improved DWA algorithm to obtain the optimal trajectory. A gradually expanding search area is established to limit the search range, reducing computational complexity. By introducing a fuzzy logic control strategy, the weight parameters of the heuristic function are dynamically adjusted based on the distribution of obstacles and the position of the unmanned watercraft, reducing path planning time. The use of the quadratic interpolation method effectively reduces the number of turning points in the path.

[0005] The article, titled "Autonomous Recovery Control Method of Unmanned Vehicle Based on Dynamic Programming Guidance," was published on December 20, 2023, in the journal "China Ship Research." It proposes a tracking control method based on dynamic programming guidance, combining parallel approach guidance (CB) with the traditional dynamic window algorithm (DWA) to guide the USV to achieve target tracking and dynamic obstacle avoidance. Specifically, in the dynamic window algorithm, the desired heading and desired speed of the parallel approach guidance (CB) method are integrated into the heading evaluation function and the speed evaluation function, thus ensuring local obstacle avoidance while achieving global optimal guidance.

[0006] The traditional DWA method is mainly designed for tracking static path points. It has a higher adaptability when combined with simpler pure pursuit guidance or parallel approach guidance algorithms. However, in the specific mission scenario of the present invention, if the USV is located in front of or in front of the UMV, these DWA methods based on conventional guidance methods are no longer applicable because they cannot ensure that the headings of the two are consistent when they meet, resulting in recovery failure. When trying to integrate more complex guidance algorithms into the traditional DWA method, its limitations gradually become apparent: (1) The traditional DWA method focuses on matching the heading of the USV's optimal simulated trajectory at the end point with the guidance strategy, but ignores the front and middle parts of the simulated trajectory. This may cause the algorithm to pursue the matching of the heading of the trajectory end point with the guidance strategy, thereby setting the end point of the trajectory at an undesirable position. (2) When the USV is tracking the UMV, its speed (linear speed) needs to be dynamically adjusted according to the changes in the relative positions of the two, so as to ensure that the speeds of the USV and UMV are consistent when they approach. When simulating USV trajectories, traditional DWA methods usually treat the sampled linear velocity as a constant linear velocity for the simulated trajectory, ignoring the dynamic variability of the USV linear velocity during autonomous recovery missions, resulting in distorted predicted trajectories. (3) Traditional dynamic window methods mainly focus on tracking and avoiding obstacles for USVs at static waypoints, but lack the ability to effectively predict and flexibly respond to the future motion trajectory of dynamic targets, resulting in detours and affecting recovery efficiency. Summary of the Invention

[0007] The purpose of the present invention is to provide an improved dynamic window local path planning method, program, device and storage medium for an unmanned boat for autonomous recovery of unmanned ocean vehicles.

[0008] An improved dynamic window local path planning method for an unmanned boat for autonomous recovery of an unmanned ocean vehicle includes the following steps:

[0009] The unmanned boat obtains its own current status information, the status information of the unmanned ocean vehicle and the distribution of obstacles;

[0010] According to the speed and acceleration constraints of the unmanned boat, the linear velocity sampling space and angular velocity sampling space of the unmanned boat are constructed, and the linear velocity sampling space and angular velocity sampling space are uniformly sampled to obtain multiple sets of dynamic window sampling solutions;

[0011] For each set of dynamic window sampling solutions, the position of the next trajectory point is predicted based on the motion model of the unmanned boat. Based on the distribution of obstacles at the current moment, dynamic window sampling solutions with potential collision risks are eliminated.

[0012] For the remaining dynamic window sampling solutions, multiple trajectory points are continuously predicted at a fixed time step starting from the current position of the unmanned boat. A dual dynamic window prediction and tracking mechanism is introduced to predict the trajectory of the unmanned boat and the unmanned ocean vehicle in parallel. The speed dynamic constraint algorithm used for autonomous recovery is integrated into the unmanned boat trajectory prediction process. The linear speed at the predicted trajectory point is dynamically adjusted according to the relative position of the unmanned boat and the unmanned ocean vehicle. The heading guidance algorithm used for autonomous recovery and the speed dynamic constraint algorithm are integrated into the dynamic window evaluation algorithm to evaluate the predicted trajectory of the unmanned boat.

[0013] The dynamic window sampling solution with the largest evaluation score is used as the control for the next path planning of the unmanned boat. After the unmanned boat moves for one time step, if the unmanned boat is not recovered by the unmanned ocean vehicle, the above steps are repeated to continue the path planning.

[0014] Furthermore, the state information of the unmanned boat at the current moment t is obtained as follows:

[0015] (x S (t),y S (t),ψ S (t),u S (t),r S (t))

[0016] The state information of the unmanned ocean vehicle obtained by the unmanned boat at the current time t is:

[0017] (x M (t),y M (t),ψ M (t),u M (t),r M (t))

[0018] Where (x, y) is the horizontal coordinate, ψ is the heading angle, u is the linear velocity, and r is the angular velocity.

[0019] Furthermore, the linear velocity sampling space and angular velocity sampling space of the unmanned boat are constructed according to the speed constraint and acceleration constraint of the unmanned boat, specifically:

[0020] Speed ​​Constraint:

[0021] u i ∈[u Smin ,u Smax ],r i ∈[r Smin ,r Smax ]

[0022] Acceleration constraints:

[0023] u i ∈[u S (k)-a Smax Δt,u S (k)+a Smax Δt], r i ∈[r S (k)-a Srmax Δt,r S (k)+a Srmax Δt]

[0024] Among them, u Smin with u Smax is the minimum and maximum linear speed of the unmanned boat, r Smin With r Smax is the minimum and maximum angular velocity of the unmanned boat, a Smax is the maximum linear acceleration of the unmanned boat, a Srmax is the maximum angular acceleration of the unmanned boat; Δt is the time step;

[0025] Uniformly sample m groups of linear velocity samples in the linear velocity sampling space, and sample n groups of angular velocity samples in the angular velocity sampling space to obtain m×n groups of sampling solutions {(u i ,r i )|i=1,2,…,m×n},(u i ,r i ) is the i-th group of dynamic window sampling solutions of the unmanned boat.

[0026] Furthermore, for each set of dynamic window sampling solutions (u i ,r i ), predict the position of the next trajectory point (x i (t+Δt),y i (t+Δt)), according to the distribution of obstacles at the current moment, the dynamic window sampling solution (u i ,r i );

[0027] ψ i (t+Δt)=ψ S (t)+r i Δt

[0028] xi (t+Δt)=x S (t)+u i ·cos(ψ i (t+Δt))·Δt

[0029] y i (t+Δt)=y S (t)+u i ·sin(ψ i (t+Δt))·Δt

[0030] Furthermore, the speed dynamic constraint algorithm for autonomous recovery is integrated into the unmanned boat trajectory prediction process, and the linear speed at the predicted trajectory point is dynamically adjusted according to the relative position of the unmanned boat and the unmanned ocean vehicle, specifically:

[0031] According to the speed dynamic constraint algorithm for autonomous recovery, the constraint speed u of the unmanned boat is calculated lim (j);

[0032] u lim (j) = min{u i ,f ui (j)}

[0033]

[0034]

[0035] Where j is the index of the predicted trajectory point; Δr is the speed control coefficient; (x Si (j-1),y Si (j-1)) is the solution based on dynamic window sampling (u i ,r i ) predicted position of the unmanned boat at the j-1th trajectory point; (x M (j-1),y M (j-1)) is the predicted position of the unmanned ocean vehicle at the j-1th trajectory point;

[0036] The linear velocity u of the unmanned boat at the jth trajectory point Si (j) The prediction is:

[0037] u Si (j) = u Si (j-1)+min{u′ Si (j),a max Δt}

[0038] u′ Si (j)=max{(u lim (j)-u Si (j-1)),-a max Δt}

[0039] Furthermore, the dual dynamic window prediction and tracking mechanism is introduced to predict the trajectory of the unmanned boat and the trajectory of the unmanned ocean vehicle in parallel, specifically:

[0040] According to the motion model of the unmanned boat, the position of the unmanned boat (x Si (j),y Si (j)) and heading angle ψ Si (j);

[0041] x Si (j) = x Si (j-1)+u Si (j)·cos(ψ Si (j-1))·Δt

[0042] y Si (j) = y Si (j-1)+u Si (j)·sin(ψ Si (j-1))·Δt

[0043] ψ Si (j) = ψ S (j-1)+r i Δt

[0044] According to the motion model of the unmanned ocean vehicle, the position of the unmanned ocean vehicle (x Si (j),y Si (j)) and heading angle ψ Si (j);

[0045] x M (j) = x M (j-1)+u M (t)·cos(ψ M (j-1))·Δt

[0046] y M (j) = y M (j-1)+u M (t)·sin(ψ M (j-1))·Δt

[0047] ψ M (j) = ψ M (j-1)+r M (t)Δt

[0048] Among them, ψ Si (j) and ψ M (j) is the predicted heading angle of the unmanned boat and the unmanned ocean vehicle at the jth trajectory point.

[0049] Furthermore, the heading guidance algorithm for autonomous recovery and the speed dynamic constraint algorithm are integrated into the dynamic window evaluation algorithm to evaluate the predicted trajectory of the unmanned boat, specifically:

[0050] According to the heading guidance algorithm for autonomous recovery, the unmanned boat and the unmanned ocean vehicle are expected to meet at the same heading angle, and the expected heading angle ψ at each trajectory point is calculated. dSi (j);

[0051] ψ dSi (j) = ψ fi (j)+ψ M (j)

[0052] Among them, ψ fi (j) is the relative heading angle; the longitudinal tracking error X ei (j) and lateral tracking error Y ei (j) Weighted, weight is K f ; K f X ei (j) and K f Y ei (j) Input into the fuzzy guidance system, according to K f X ei (j) and K f Y ei (j) belongs to the fuzzy description, and obtains the relative heading angle ψ fi (j);

[0053] X ei (j) = (x Si (j)-x M (j))cos(ψ M (j))+(y Si (j)-y M (j))sin(ψ M (j))

[0054] Y ei (j)=(y Si (j)-y M (j))cos(ψ M (j))-(x Si (j)-x M (j))sin(ψ M (j))

[0055] Calculate the azimuth deviation evaluation function f head (i);

[0056]

[0057] Among them, the function eψ (ψ1, ψ2) is the difference in radians between ψ1 and ψ2;

[0058] Calculate the speed deviation evaluation function f vel (i);

[0059]

[0060] The closest distance between the unmanned boat and the obstacle in N trajectory points is used as the collision avoidance evaluation function f dist (i);

[0061] f head (i) f vel (i) and f dist (i) After normalization, weighted aggregation is performed to obtain the dynamic window sampling solution (u i ,r i ) corresponding evaluation score J i ;

[0062] J i =α1σ(f head (i))+α2σ(f vel (i))+α3σ(f dist (i))

[0063] Among them, α1, α2, and α3 are weight coefficients, α1>0, α2>0, and α3>0; σ(·) represents the normalization operation.

[0064] A computer device / equipment / system includes a memory, a processor, and a computer program stored in the memory, wherein the processor executes the computer program to implement the steps of the above-mentioned improved dynamic window local path planning method for unmanned boats for autonomous recovery of unmanned ocean vehicles.

[0065] A computer-readable storage medium stores a computer program / instruction, which, when executed by a processor, implements the steps of the above-mentioned improved dynamic window local path planning method for unmanned boats for autonomous recovery of unmanned ocean vehicles.

[0066] A computer program product includes a computer program / instruction, which, when executed by a processor, implements the steps of the above-mentioned improved dynamic window local path planning method for unmanned boats for autonomous recovery of unmanned ocean vehicles.

[0067] The beneficial effects of the present invention are:

[0068] The present invention introduces a dual dynamic window prediction and tracking mechanism, which simulates the trajectory of the unmanned boat while predicting the future trajectory of the unmanned ocean vehicle in parallel. The expected linear velocity and expected angular velocity of the unmanned boat are calculated by comprehensively considering the simulated trajectories of the two, effectively reducing unnecessary detours and improving the efficiency of the recovery operation. The present invention integrates the speed dynamic constraint algorithm into the unmanned boat trajectory simulation process, and dynamically adjusts the linear velocity at the simulated trajectory point according to the relative position of the unmanned boat and the unmanned ocean vehicle, overcoming the limitation of the traditional dynamic window method of simulating with a constant linear velocity, making the predicted trajectory of the unmanned boat closer to the actual autonomous recovery mission scenario. The present invention integrates the bow guidance algorithm and the speed dynamic constraint algorithm for autonomous recovery into the dynamic window evaluation algorithm, and evaluates each trajectory point of the unmanned boat simulated trajectory to ensure that the entire trajectory can fit the bow guidance algorithm and the speed dynamic constraint algorithm, solving the problem of incompatibility between the traditional dynamic window method and the recovery guidance algorithm. BRIEF DESCRIPTION OF THE DRAWINGS

[0069] Figure 1 This is the overall process architecture diagram of the present invention.

[0070] Figure 2 This is a schematic diagram of the present invention.

[0071] Figure 3 This is a schematic diagram of the dual dynamic window prediction and tracking mechanism in the present invention and a schematic diagram of the trajectory simulation of the traditional dynamic window method.

[0072] Figure 4 Schematic diagram of the fuzzy guidance algorithm in an embodiment of the present invention.

[0073] Figure 5 Graph showing the fuzzy input membership function in an embodiment of the present invention.

[0074] Figure 6 1 is a guidance vector diagram of the fuzzy guidance algorithm in an embodiment of the present invention.

[0075] Figure 7 This is a comparison chart of the simulation of the present invention and the traditional dynamic window method (based on fuzzy guidance). DETAILED DESCRIPTION

[0076] The present invention will be further described below with reference to the accompanying drawings.

[0077] The present invention provides an improved dynamic window local path planning method for an unmanned boat for autonomous recovery of an unmanned ocean vehicle, which specifically includes the following steps:

[0078] Step 1: The unmanned boat obtains its own status information at the current time t (x S (t),y S (t),ψ S (t),u S(t),r S (t)), status information of unmanned ocean vehicles (x M (t),y M (t),ψ M (t),u M (t),r M (t)), distribution of obstacles;

[0079] Where (x, y) is the horizontal coordinate, ψ is the heading angle, u is the linear velocity, and r is the angular velocity;

[0080] Step 2: According to the speed constraint and acceleration constraint of the unmanned boat, the linear velocity sampling space and angular velocity sampling space of the unmanned boat are constructed. In the linear velocity sampling space, m groups of linear velocity samples are uniformly sampled, and in the angular velocity sampling space, n groups of angular velocity samples are sampled. The m×n groups of sampling solutions {(u i ,r i )|i=1,2,…,m×n},(u i ,r i ) is the i-th group of dynamic window sampling solutions for the unmanned boat;

[0081] Speed ​​Constraint:

[0082] u i ∈[u Smin ,u Smax ],r i ∈[r Smin ,r Smax ]

[0083] The acceleration constraint is:

[0084] u i ∈[u S (k)-a Smax Δt,u S (k)+a Smax Δt], r i ∈[r S (k)-a Srmax Δt,r S (k)+a Srmax Δt]

[0085] Among them, u Smin with u Smax is the minimum and maximum linear speed of the unmanned boat, r Smin With r Smax is the minimum and maximum angular velocity of the unmanned boat, a Smax is the maximum linear acceleration of the unmanned boat, a Srmax is the maximum angular acceleration of the unmanned boat;

[0086] Step 3: For each set of dynamic window sampling solutions (ui ,r i ), predict the position of the next trajectory point (x i (t+Δt),y i (t+Δt)), according to the distribution of obstacles at the current moment, the dynamic window sampling solution (u i ,r i );

[0087] ψ i (t+Δt)=ψ S (t)+r i Δt

[0088] x i (t+Δt)=x S (t)+u i ·cos(ψ i (t+Δt))·Δt

[0089] y i (t+Δt)=y S (t)+u i ·sin(ψ i (t+Δt))·Δt

[0090] Where Δt is the time step;

[0091] Step 4: For the remaining dynamic window sampling solutions (u i ,r i ), from the current position of the unmanned boat at time t (x S (t),y S (t)) starts predicting N trajectory points with a time step of Δt; introduces a dual dynamic window prediction and tracking mechanism to predict the trajectory of the unmanned boat and the unmanned ocean vehicle in parallel; integrates the speed dynamic constraint algorithm for autonomous recovery into the unmanned boat trajectory prediction process, dynamically adjusts the linear speed at the predicted trajectory point according to the relative position of the unmanned boat and the unmanned ocean vehicle, integrates the heading guidance algorithm for autonomous recovery and the speed dynamic constraint algorithm into the dynamic window evaluation algorithm, and evaluates the predicted trajectory of the unmanned boat;

[0092] For the dynamic window sampling solution (u i ,r i ), the method for predicting N trajectory points is as follows:

[0093] Step 4.1: Initialize j = 1, let x Si (0) = x S (t), y Si (0) = y S (t), ψ Si (0) = ψS (t),u Si (0) = u S (t), x M (0) = x M (t), y M (0) = y M (t), ψ M (0) = ψ M (t);

[0094] Step 4.2: Based on the status information of the unmanned boat and the unmanned ocean vehicle at the previous path point, the speed dynamic constraint algorithm for autonomous recovery is used to calculate the constrained speed u of the unmanned boat. lim (j);

[0095] u lim (j) = min{u i ,f ui (j)}

[0096]

[0097] Among them, Δr is the speed regulation coefficient;

[0098] Step 4.3: Update the speed u of the unmanned boat Si (j);

[0099] u Si (j) = u Si (j-1)+min{u′ Si (j),a max Δt}

[0100] u′ Si (j)=max{(u lim (j)-u Si (j-1)),-a max Δt}

[0101] Step 4.3: Update the position of the unmanned boat (x Si (j),y Si (j)) and heading angle ψ Si (j);

[0102] x Si (j) = x Si (j-1)+u Si (j)·cos(ψ Si (j-1))·Δt

[0103] y Si (j) = y Si (j-1)+u Si (j)·sin(ψSi (j-1))·Δt

[0104] ψ Si (j) = ψ S (j-1)+r i Δt

[0105] Step 4.4: Update the position of the unmanned ocean vehicle (x Si (j),y Si (j)) and heading angle ψ Si (j);

[0106] x M (j) = x M (j-1)+u M (t)·cos(ψ M (j-1))·Δt

[0107] y M (j) = y M (j-1)+u M (t)·sin(ψ M (j-1))·Δt

[0108] ψ M (j) = ψ M (j-1)+r M (t)Δt

[0109] Step 4.5: If j < N, set j = j + 1 and return to step 4.2; otherwise, output the dynamic window sampling solution (u i ,r i ) corresponding to the state information of the unmanned boat at N trajectory points (x Si (j),y Si (j),ψ Si (j),u Si (j)) and the pose information of the unmanned ocean vehicle (x M (j),y M (j),ψ M (j));

[0110] For the dynamic window sampling solution (u i ,r i ) is evaluated based on the trajectory of N trajectory points corresponding to , specifically:

[0111] Step 4.6: Calculate the desired heading angle ψ at each trajectory point based on the heading guidance algorithm used for autonomous recovery dSi (j);

[0112] Step 4.6.1: Calculate the longitudinal tracking error X at each trajectory pointei (j) and lateral tracking error Y ei (j);

[0113] X ei (j) = (x Si (j)-x M (j))cos(ψ M (j))+(y Si (j)-y M (j))sin(ψ M (j))

[0114] Y ei (j)=(y Si (j)-y M (j))cos(ψ M (j))-(x Si (j)-x M (j))sin(ψ M (j))

[0115] Step 4.6.2: Longitudinal tracking error X ei (j) and lateral tracking error Y ei (j) Weighted, weight is K f ; K f X ei (j) and K f Y ei (j) Input into the fuzzy guidance system, according to K f X ei (j) and K f Y ei (j) belongs to the fuzzy description, and obtains the relative heading angle ψ fi (j);

[0116] Step 4.6.3: Assuming that the UAV and the UUV will rendezvous with the same heading angle as expected, calculate the expected heading angle ψ at each trajectory point. dSi (j);

[0117] ψ dSi (j) = ψ fi (j)+ψ M (j)

[0118] Step 4.7: Calculate the azimuth deviation evaluation function f head (i);

[0119]

[0120] Among them, the function e ψ (ψ1, ψ2) is the difference in radians between ψ1 and ψ2;

[0121] Step 4.8: Calculate the speed deviation evaluation function f vel (i);

[0122]

[0123] Step 4.9: Take the shortest distance between the UAV and the obstacle in the N trajectory points as the collision avoidance evaluation function f dist (i);

[0124] Step 4.10: Put f head (i) f vel (i) and f dist (i) After normalization, weighted aggregation is performed to obtain the dynamic window sampling solution (u i ,r i ) corresponding evaluation score J i ;

[0125] J i =α1σ(f head (i))+α2σ(f vel (i))+α3σ(f dist (i))

[0126] Among them, α1, α2, and α3 are weight coefficients, α1>0, α2>0, and α3>0; σ(·) represents the normalization operation;

[0127] Step 5: The corresponding evaluation score J i The largest dynamic window sampling solution (u i ,r i ) is used as the control for the next path planning of the unmanned boat. After Δt time, if the unmanned boat is not recovered by the unmanned ocean vehicle, return to step 1.

[0128] Example 1:

[0129] Reference Figure 1 and Figure 2 To specifically describe this implementation, we first design a heading guidance algorithm and a speed dynamic constraint algorithm that can guide the USV to rendezvous with the UMV in a suitable attitude;

[0130] (1) Heading guidance algorithm

[0131] In this embodiment, the fuzzy guidance proposed in the dissertation "Study on the Compliant Guidance and Robust Control Method for Unmanned Vehicles for UUV Retraction and Deployment" is used as an example to illustrate a possible heading guidance algorithm as a guidance algorithm for the improved dynamic window method. The heading guidance algorithm in actual application is not limited to this algorithm. The following is an introduction to fuzzy guidance. The fuzzy guidance principle diagram is shown in Figure 4 . Let the USV pose point be P USV, then its horizontal coordinate is (P USV [x],P USV [y]); let the UMV pose point be P UMV , then its horizontal coordinate is (P UMV [x],P UMV [y]), its heading angle is P UMV [ψ], then the longitudinal tracking error X e and lateral tracking error Y e The algorithm is:

[0132] X e =(P USV [x]-P UMV [x])cos(P UMV [ψ])+(P USV [y]-P UMV [y])sin(P UMV [ψ])

[0133] Y e =(P USV [y]-P UMV [y])cos(P UMV [ψ])-(P USV [x]-P UMV [x])sin(P UMV [ψ])

[0134] The fuzzy system is based on the weighted longitudinal error K f X e And the weighted lateral error K f Y e As input, the relative heading angle ψ f is the output, where K f is the parameter that determines the guidance range. Define K f Y e The fuzzy description is {NB, NM, NS, ZE, PS, PM, PB}, that is, {negative large, negative medium, negative small, zero, positive small, positive medium, positive large}; define K f X e The fuzzy description of K is {OV,HO,NE,FA,VF}, that is, {over, zero, near, middle, far}. f X e and K f Y e The membership curve is as follows Figure 5 As shown, when K f X e When it is greater than 0, there is only one fuzzy description of OV and the membership is 1 ( Figure 5 The fuzzy rules are shown in the following table:

[0135]

[0136] So a set of input (K f X e ,K f Y e ) is given to the fuzzy guided controller, if it is assumed that the set of inputs is associated with m rules, then the form of its i-th fuzzy rule is as follows:

[0137] Rule i:If K f X e is A i and K f Y e is B i ,thenψ f is C i (i=1…m)

[0138] The calculation method of the weight coefficient of each rule in the dual-input single-output fuzzy inference model is the product method, and the output of the controller adopts the weighted average method:

[0139] ω i =A i ·B i

[0140]

[0141] Where A i and B i Corresponding to the input information in the i-th rule (K f X e ,K f Y e ) respectively correspond to the membership degree, ω i is the weight corresponding to the rule, C i is the output of this rule, ψ f The inference results of the controller. Figure 6 The guidance vector field generated by this fuzzy guidance algorithm is shown. The center of the vector field represents the UMV's position, and the UMV's heading is aligned with the positive x-axis. When the USV is at different positions and distances from the UMV, the fuzzy guidance algorithm provides the USV with the corresponding desired heading, allowing the USV to ultimately rendezvous with the UMV at the same heading, thus completing recovery. The final desired heading angle algorithm is:

[0142] ψ d =ψ f +P UMV [ψ]

[0143] In summary, the above fuzzy guidance algorithm can be expressed as:

[0144] ψd =f ψ (P USV ,P UMV )

[0145] Among them, ψ d is the desired heading, P USV Represents a USV trajectory point, P UUV Representing a UMV trajectory point, the information contained in a horizontal plane trajectory point P should at least include the abscissa P[x], ordinate P[y], heading angle P[ψ], and linear speed P[u].

[0146] (2) Speed ​​dynamic constraint algorithm

[0147] In this embodiment, a speed control algorithm from the master's thesis "Study on the compliant guidance and robust control method for unmanned boats for UUV deployment" is used as an example to illustrate a possible speed dynamic constraint algorithm as a guidance algorithm for the improved dynamic window method. The speed dynamic constraint algorithm in actual application is not limited to this algorithm. Let the USV posture point be P USV , then its horizontal coordinate is (P USV [x],P USV [y]), the speed is P USV [u]; let the UMV pose point be P UMV , then its horizontal coordinate is (P UMV [x],P UMV [y]), the UMV speed is P USV [u]. Defines the constrained speed u lim In the autonomous recovery mission, the speed of the unmanned boat needs to be adjusted to the constrained speed or below, ensuring that when the distance between the USV and the UMV decreases, the speed of the USV gradually approaches the speed of the UMV, thereby achieving synchronous navigation of the two and ensuring the success rate of recovery. The speed adjustment algorithm is:

[0148]

[0149] where u max is the maximum speed of the unmanned boat, Δr is the speed control coefficient, and D is the P USV With P UMV The distance is calculated as follows:

[0150]

[0151] In the above algorithm, the closer the distance between USV and UMV is, the higher the constraint speed u lim When the USV and UMV positions coincide, the USV constrained speed is consistent with the UMV actual speed, making it easier for the unmanned boat to autonomously recover the unmanned ocean vehicle.

[0152] Finally, the above speed dynamic constraint algorithm can be expressed as:

[0153] u lim =f u (P USV ,P UMV )

[0154] Among them, P USV Represents a USV trajectory point, P UMV Representing a UMV trajectory point, the information contained in a horizontal plane trajectory point P should at least include the abscissa P[x], ordinate P[y], heading angle P[ψ], and linear speed P[u].

[0155] The speed dynamic constraint algorithm is designed to limit the speed of the USV approaching the UMV and improve the success rate of autonomous recovery. Introducing the speed dynamic constraint algorithm into the single-step update rule of the USV simulation trajectory can make the USV simulation trajectory more realistic.

[0156] like Figure 7 This paper compares the simulation results of the traditional dynamic window method based on a type of fuzzy guidance algorithm and a type of speed control algorithm with the improved dynamic window method in the present invention. The improved dynamic window method can avoid obstacles and complete the autonomous recovery task. According to the simulation results, the following conclusions can be drawn: (1) The traditional dynamic window method only uses the end position of the simulated trajectory as the input of the azimuth deviation evaluation function, and uses a constant linear velocity when simulating the trajectory. As a result, even if the azimuth angle of the end of the USV simulated trajectory matches the fuzzy guidance, it still cannot complete the recovery task. In contrast, the improved dynamic window method can ensure that the entire trajectory is closely aligned with the guidance algorithm and can successfully complete the recovery task. (2) Since the improved dynamic window method synchronously predicts the trajectories of the USV and the UMV, and introduces a speed dynamic constraint algorithm when simulating the USV trajectory, the simulated trajectory of the USV is more accurate. Therefore, after the USV and the UMV meet, the USV can still stably track the UMV, ensuring the recovery success rate.

[0157] The foregoing description is merely a preferred embodiment of the present invention and is not intended to limit the present invention. Those skilled in the art will readily appreciate that various modifications and variations of the present invention are possible. Any modifications, equivalent substitutions, or improvements made within the spirit and principles of the present invention are intended to be within the scope of protection of the present invention.

Claims

1. An improved dynamic window local path planning method for unmanned boats for autonomous recovery of unmanned ocean vehicles, characterized by: The unmanned boat obtains its own current status information, the status information of the unmanned ocean vehicle and the distribution of obstacles; According to the speed and acceleration constraints of the unmanned boat, the linear velocity sampling space and angular velocity sampling space of the unmanned boat are constructed, and the linear velocity sampling space and angular velocity sampling space are uniformly sampled to obtain multiple sets of dynamic window sampling solutions; For each set of dynamic window sampling solutions, the position of the next trajectory point is predicted based on the motion model of the unmanned boat. Based on the distribution of obstacles at the current moment, dynamic window sampling solutions with potential collision risks are eliminated. For the remaining dynamic window sampling solutions, multiple trajectory points are continuously predicted at a fixed time step starting from the current position of the unmanned boat. A dual dynamic window prediction and tracking mechanism is introduced to predict the trajectory of the unmanned boat and the unmanned ocean vehicle in parallel. The speed dynamic constraint algorithm used for autonomous recovery is integrated into the unmanned boat trajectory prediction process. The linear speed at the predicted trajectory point is dynamically adjusted according to the relative position of the unmanned boat and the unmanned ocean vehicle. The heading guidance algorithm used for autonomous recovery and the speed dynamic constraint algorithm are integrated into the dynamic window evaluation algorithm to evaluate the predicted trajectory of the unmanned boat. The dynamic window sampling solution with the largest evaluation score is used as the control for the next path planning of the unmanned boat. After the unmanned boat moves for one time step, if the unmanned boat is not recovered by the unmanned ocean vehicle, the above steps are repeated to continue the path planning.

2. The improved dynamic window local path planning method for an unmanned boat for autonomous recovery of unmanned ocean vehicles according to claim 1 is characterized by: The state information of the unmanned boat at the current moment t is: (x S (t),y S (t),ψ S (t),u S (t),r S (t)) The state information of the unmanned ocean vehicle obtained by the unmanned boat at the current time t is: (x M (t),y M (t),ψ M (t),u M (t),r M (t)) Where (x, y) is the horizontal coordinate, ψ is the heading angle, u is the linear velocity, and r is the angular velocity.

3. The improved dynamic window local path planning method for an unmanned boat for autonomous recovery of unmanned ocean vehicles according to claim 2 is characterized by: According to the speed constraint and acceleration constraint of the unmanned boat, the linear velocity sampling space and angular velocity sampling space of the unmanned boat are constructed, specifically: Speed ​​Constraint: u i ∈[u Smin ,u Smax ],r i ∈[r Smin ,r Smax ] Acceleration constraints: u i ∈[u S (k)-a Smax Δt,u S (k)+a Smax Δt],r i ∈[r S (k)-a Srmax Δt,r S (k)+a Srmax Δt] Among them, u Smin with u Smax is the minimum and maximum linear speed of the unmanned boat, r Smin With r Smax is the minimum and maximum angular velocity of the unmanned boat, a Smax is the maximum linear acceleration of the unmanned boat, a Srmax is the maximum angular acceleration of the unmanned boat; Δt is the time step; Uniformly sample m groups of linear velocity samples in the linear velocity sampling space, and sample n groups of angular velocity samples in the angular velocity sampling space to obtain m×n groups of sampling solutions {(u i ,r i )|i=1,2,…,m×n},(u i ,r i ) is the i-th group of dynamic window sampling solutions of the unmanned boat.

4. The improved dynamic window local path planning method for an unmanned boat for autonomous recovery of unmanned ocean vehicles according to claim 3 is characterized by: For each set of dynamic window sampling solutions (u i ,r i ), predict the position of the next trajectory point (x i (t+Δt),y i (t+Δt)), according to the distribution of obstacles at the current moment, the dynamic window sampling solution (u i ,r i ); ψ i (t+Δt)=ψ S (t)+r i Δt x i (t+Δt)=x S (t)+u i ·cos(ψ i (t+Δt))·Δt y i (t+Δt)=y S (t)+u i ·sin(ψ i (t+Δt))·Δt。 5. The improved dynamic window local path planning method for an unmanned boat for autonomous recovery of unmanned ocean vehicles according to claim 3 is characterized by: The dynamic speed constraint algorithm for autonomous recovery is integrated into the unmanned boat trajectory prediction process, and the linear speed at the predicted trajectory point is dynamically adjusted according to the relative position of the unmanned boat and the unmanned ocean vehicle. Specifically, According to the speed dynamic constraint algorithm for autonomous recovery, the constraint speed u of the unmanned boat is calculated lim (j); you lim (j) = min{u i ,f ui (j)} Where j is the index of the predicted trajectory point; Δr is the speed control coefficient; (x Si (j-1),y Si (j-1)) is the solution based on dynamic window sampling (u i ,r i ) predicted position of the unmanned boat at the j-1th trajectory point; (x M (j-1),y M (j-1)) is the predicted position of the unmanned ocean vehicle at the j-1th trajectory point; The linear velocity u of the unmanned boat at the jth trajectory point Si (j) The prediction is: you Si (j)=u Si (j-1)+min{u′ Si (j),a max Δt} u' Si (j)=max{(u lim (j)-u Si (j-1)),-a max Δt}。 6. The improved dynamic window local path planning method for an unmanned boat for autonomous recovery of unmanned ocean vehicles according to claim 5 is characterized by: The dual dynamic window prediction and tracking mechanism is introduced to predict the trajectory of the unmanned boat and the trajectory of the unmanned ocean vehicle in parallel. Specifically: According to the motion model of the unmanned boat, the position of the unmanned boat (x Si (j),y Si (j)) and heading angle ψ Si (j); x Si (j)=x Si (j-1)+u Si (j)·cos(ψ Si (j-1))·Δt y Si (j)=y Si (j-1)+u Si (j) sin(ψ Si (j-1))·Δt ψ Si (j)=ψ S (j-1)+r i Δt According to the motion model of the unmanned ocean vehicle, the position of the unmanned ocean vehicle (x Si (j),y Si (j)) and heading angle ψ Si (j); x M (j)=x M (j-1)+u M (t)·cos(ψ M (j-1))·Δt y M (j)=y M (j-1)+u M (t) sin(ψ M (j-1))·Δt ψ M (j)=ψ M (j-1)+r M (t)Δt Among them, ψ Si (j) and ψ M (j) is the predicted heading angle of the unmanned boat and the unmanned ocean vehicle at the jth trajectory point.

7. The improved dynamic window local path planning method for an unmanned boat for autonomous recovery of unmanned ocean vehicles according to claim 6 is characterized by: The heading guidance algorithm for autonomous recovery and the speed dynamic constraint algorithm are integrated into the dynamic window evaluation algorithm to evaluate the predicted trajectory of the unmanned boat. Specifically: According to the heading guidance algorithm for autonomous recovery, the unmanned boat and the unmanned ocean vehicle are expected to meet at the same heading angle, and the expected heading angle ψ at each trajectory point is calculated. dSi (j); ψ dSi (j)=ψ fi (j)+ψ M (j) Among them, ψ fi (j) is the relative heading angle; the longitudinal tracking error X ei (j) and lateral tracking error Y ei (j) Weighted, weight is K f ; K f X ei (j) and K f Y ei (j) Input into the fuzzy guidance system, according to K f X ei (j) and K f Y ei (j) belongs to the fuzzy description, and obtains the relative heading angle ψ fi (j); X ei (j)=(x Si (j)-x M (j))cos(ψ M (j))+(y Si (j)-y M (j))sin(ψ M (j)) Y ei (j)=(y Si (j)-y M (j))cos(ψ M (j))-(x Si (j)-x M (j))sin(ψ M (j)) Calculate the azimuth deviation evaluation function f head (i); Among them, the function e ψ (ψ1, ψ2) is the difference in radians between ψ1 and ψ2; Calculate the speed deviation evaluation function f vel (i); The closest distance between the unmanned boat and the obstacle in N trajectory points is used as the collision avoidance evaluation function f dist (i); f head (i) f vel (i) and f dist (i) After normalization, weighted aggregation is performed to obtain the dynamic window sampling solution (u i ,r i ) corresponding evaluation score J i ; J i =α1σ(f head (i))+α2σ(f vel (i))+α3σ(f dist (i)) Among them, α1, α2, and α3 are weight coefficients, α1>0, α2>0, and α3>0; σ(·) represents the normalization operation.

8. A computer device / apparatus / system comprising a memory, a processor, and a computer program stored in the memory, characterized in that: The processor executes the computer program to implement the steps of the method according to any one of claims 1 to 7.

9. A computer-readable storage medium having a computer program / instruction stored thereon, characterized in that: When the computer program / instructions are executed by a processor, the steps of the method according to any one of claims 1 to 7 are implemented.

10. A computer program product comprising a computer program / instructions, characterized in that: When the computer program / instructions are executed by a processor, the steps of the method according to any one of claims 1 to 7 are implemented.

Images