METHOD FOR TARGET ASSIGNMENT AND ROUTE PLANNING FOR MULTI-AGENT UNMANNED SURFACE VEHICLES

Abstract

The present disclosure discloses a method for target assignment and route planning for multi-agent unmanned surface vehicles (USVs). Task information is transmitted to a task allocator, which identifies unassigned targets and available USVs. Based on a comprehensive cost function considering navigation distance and turning penalties between targets and USVs, a target is assigned to each USV A path planner then generates a smooth waypoint sequence for each assigned USV and target pair, serving as the navigation path. During navigation, a navigation controller constructs a velocity optimization model according to cooperative collision avoidance and maritime boundary constraints, and controls the USVs in real-time to navigate along the waypoint sequences. The present disclosure achieves efficient matching of multiple targets and multi-agent USVs, generates smooth navigation paths satisfying constraints, and enables intelligent navigation control while meeting cooperative collision avoidance requirements and maritime boundary constraints.

Claims

1. A method for target assignment and route planning for multi-agent unmanned surface vehicles, comprising: receiving task information by a task allocator, wherein the task information comprises a task area, restricted area, parameters and state quantities of targets, and parameters and state quantities of unmanned surface vehicles; identifying a set of unassigned targets T.sub.un and a set of unmanned surface vehicles without assigned tasks U.sub.un through the parameters and state quantities of the targets and unmanned surface vehicles; assigning a corresponding target to each unmanned surface vehicle, comprising: calculating an adjusted distance d.sub.ij from each target j to each unmanned surface vehicle i: $d_{\mathrm{ij}}^{}=d_{\mathrm{ij}}+d_{\mathrm{ij}}+d_{\mathrm{ij}}$ wherein, d.sub.ij is the Euclidean distance from the i-th unmanned surface vehicle to the j-th target, d.sub.ij is an adjustment term considering performance parameters of the unmanned surface vehicle for the distance, and d.sub.ij is a penalty term for the j-th target bypassing restricted area from the i-th unmanned surface vehicle; constructing an adjusted distance matrix D.sub.mn between unmanned surface vehicles and targets by d.sub.ij, defining a sub-matrix D, DD.sub.mn, D comprising rows and columns corresponding to unassigned unmanned surface vehicles u.sub.i and targets t.sub.j, and matching the unassigned unmanned surface vehicles u.sub.i and targets t.sub.j based on a minimum element in D; wherein the unmanned surface vehicle u.sub.i belongs to the set of unmanned surface vehicles without assigned tasks U.sub.un, and the target t.sub.j belongs to the set of unassigned targets T.sub.un; wherein a process for obtaining the penalty term for each target j bypassing restricted area from the unmanned surface vehicle i is: 1) dividing a straight-line path from the unmanned surface vehicle i to the target j into N equal segments, each segment having a length of l, and endpoints of each segment being denoted as sampling points P.sub.0 . . . P.sub.k . . . P.sub.N; 2) determining whether a connecting line P.sub.kP.sub.k+1 intersects with a boundary of a restricted area F.sub.m, and if intersecting, denoting a first intersection point and a last intersection point as a.sub.k, b.sub.k respectively, and deleting sampling points between a.sub.k, b.sub.k; 3) finding a point n.sub.k on the boundary of F.sub.m that is closest to a.sub.k; 4) calculating a point a.sub.n,k obtained by extending from n.sub.k outwards along a normal vector direction away from a center point c.sub.m of F.sub.m by a distance d; 5) starting from a.sub.n,k, traversing boundary points of F.sub.m outwards by a distance d in clockwise and counter-clockwise directions respectively to obtain points a.sub.n,k+i, until a connecting line between a.sub.n,k+i and b.sub.k does not intersect with F.sub.m, and recording path lengths in clockwise and counter-clockwise directions L.sub.cw, L.sub.ccw respectively; 6) selecting a smaller value from L.sub.cw and L.sub.ccw, and denoting it as L.sub.k; 7) adding path points bypassing F.sub.m, i.e., points in L.sub.k, between the sampling points a.sub.k, b.sub.k, and repeating the steps 2 to 6 to process subsequent restricted area F.sub.m+i; 8) finally obtaining a sampling point sequence P as a path bypassing all F.sub.m, and a length thereof being $d_{\mathrm{ij}}^{};$ 9) for the sampling point sequence P, calculating a turning angle .sub.k between each adjacent sampling line segment P.sub.kP.sub.k+1 and P.sub.k+1P.sub.k+2; 10) calculating a path turning penalty term: $={.Math.}_{k=1}^{N_p-1}{}_k{{}_k}^2,{}_k$ being a turning penalty parameter; and 11) the penalty term d.sub.ij for each target j bypassing restricted area from the unmanned surface vehicle i is: d.sub.ij=d.sub.ij+; for the matched unmanned surface vehicles and corresponding targets, calculating to obtain a waypoint sequence P.sub.ij bypassing restricted area from the unmanned surface vehicle u.sub.i to the target t.sub.j, and interpolating P.sub.ij to generate a new waypoint sequence P.sub.ij; using the sequence P.sub.ij as a navigation waypoint sequence from the unmanned surface vehicle u.sub.i to the target t.sub.j, and sending and storing it in a navigation system of a corresponding unmanned surface vehicle; inputting parameters and state quantities of the unmanned surface vehicle u.sub.i, an obstacle set Obs.sub.i, restricted area F.sub.m, a task area L, and an expected velocity vector v.sub.des of the unmanned surface vehicle u.sub.i to its next waypoint P.sub.iP.sub.ij into an optimizer, and solving to obtain a velocity control quantity; and using the velocity control quantity to drive the unmanned surface vehicle u.sub.i to travel towards the target waypoint P.sub.i, and after reaching the waypoint, deleting P.sub.i from the waypoint sequence P.sub.ij until reaching a last waypoint in the waypoint sequence P.sub.ij, i.e., reaching a task area of the target t.sub.j, and after performing a task, adding the unmanned surface vehicle u.sub.i to U.sub.un again, and restarting the step 2 until all tasks are completed.

2. The method for target assignment and route planning for multi-agent unmanned surface vehicles according to claim 1, wherein the adjustment term considering performance parameters of the unmanned surface vehicle for the distance is calculated according to the following formula: $d_{\mathrm{ij}}={}_1\frac{.Math.v_i.Math.\cos {}_{\mathrm{ij}}-V_i}{V_i}+{}_2\frac{.Math.{}_i-{}_{\mathrm{ij}}.Math.}{{}_i}$ wherein, v.sub.i is a velocity vector of the i-th unmanned surface vehicle, .sub.i, is a direction of the velocity vector, V.sub.i is a maximum speed, .sub.i is a maximum turning rate, .sub.ij is an azimuth angle from the i-th unmanned surface vehicle pointing to the j-th target, and .sub.1, .sub.2 are weighting coefficients.

3. The method for target assignment and route planning for multi-agent unmanned surface vehicles according to claim 1, wherein interpolating P.sub.ij to generate a new waypoint sequence P.sub.ij specifically adopts the following method: 1) constructing a cubic B-spline curve C(u) by using P.sub.ij as a control point sequence; 2) defining a turning energy function E.sub.b={umlaut over (C)}.sup.2(u)du, representing turning energy; 3) defining a route length function E.sub.l=|(u)|du, representing route length; 4) constructing an objective function E=w.sub.1E.sub.b+w.sub.2E.sub.l, wherein w.sub.1, w.sub.2 are weighting coefficients; 5) minimizing the objective function by adjusting w.sub.1, w.sub.2 values to obtain a smooth curve C*(u); 6) sampling the C*(u) curve to generate a new waypoint sequence P.sub.tij; and 7) deleting points in the new waypoint sequence P.sub.tij that are within restricted area or in a task area to obtain the sequence P.sub.ij.

4. The method for target assignment and route planning for multi-agent unmanned surface vehicles according to claim 1, wherein an objective function of the optimizer is: $J\left(v\right)={.Math.v_{\mathrm{des}}-v.Math.}^2+c_{\mathrm{penalty}}\left(v\right)$ wherein, v is an optimization variable, C.sub.penalty is a penalty term.

5. The method for target assignment and route planning for multi-agent unmanned surface vehicles according to claim 4, wherein constraint conditions constructed by the optimizer are: C.sub.1: |v|V.sub.i, ensuring that a speed is less than a maximum speed V.sub.i; C.sub.2: p.sub.next.Math.F.sub.m, ensuring that a future position is not within restricted area F.sub.m; and C.sub.3: p.sub.nextL, ensuring that a future position is within a task area L; wherein, P.sub.next=P.sub.i+v.Math.dt is a next moment position calculated according to a velocity control quantity v.

6. The method for target assignment and route planning for multi-agent unmanned surface vehicles according to claim 5, comprising solving for a velocity control quantity v* that minimizes the objective function J(v): $\underset{v}{\min }J\left(v\right)s.t.c_1,c_2,c_3$ using the solved v* as the velocity control quantity.

7. The method for target assignment and route planning for multi-agent unmanned surface vehicles according to claim 6, wherein in each time step dt of solving by the optimizer, for each unmanned surface vehicle u.sub.i, checking whether there are other unmanned surface vehicles u.sub.j or targets t.sub.k within a search radius, and if yes, adding the other unmanned surface vehicles u.sub.j or targets t.sub.k to an obstacle set Obs.sub.i.

8. The method for target assignment and route planning for multi-agent unmanned surface vehicles according to claim 1, wherein the parameters and state quantities of the unmanned surface vehicles comprise a unique identifier, a maximum speed, a turning rate, a search radius, whether a task has been assigned, a current position, and a current velocity vector; the parameters and state quantities of the targets comprise a unique identifier, whether it has been assigned, a work area radius, and a current position.

9. The method for target assignment and route planning for multi-agent unmanned surface vehicles according to claim 1, wherein the task area is a convex polygon defined by a plurality of two-dimensional coordinates meeting a WGS84 standard; and the restricted area are convex polygons defined by a plurality of two-dimensional coordinates meeting the WGS84 standard.

Description

BRIEF DESCRIPTION OF THE DRAWINGS

[0020] In order to facilitate a better understanding of the present application, various forms thereof given as examples will now be described with reference to the accompanying drawings, in which:

[0021] FIG. 1 is a flowchart of target assignment and route planning for multi-agent unmanned surface vehicles according to the present disclosure;

[0022] FIG. 2 is a schematic diagram of obtaining a penalty term for the target j bypassing restricted area from the unmanned surface vehicle i according to the present disclosure;

[0023] FIG. 3 is a diagram of a real machine operation interface of the method according to the present disclosure.

[0024] The accompanying drawings are for illustrative purposes only and are not intended to limit the scope of protection of the present application in any way or form.

DETAILED DESCRIPTION OF THE EMBODIMENTS

[0025] Hereinafter, various embodiments shown in the accompanying drawings will be described in detail with reference to the accompanying drawings. However, these embodiments do not limit the present disclosure, and structural, methodological, or functional changes made by those of ordinary skill in the art based on these embodiments are all included in the protection scope of the present disclosure.

[0026] As shown in FIG. 1, a method for target assignment and route planning for multi-agent unmanned surface vehicles of the present disclosure includes the following steps:

[0027] Step 1., an upper-level command controller sends task information to a task allocator, wherein the task information includes a task area, restricted area, related parameters of targets, and related parameters of unmanned surface vehicles; the task allocator performs the following processing on the received task information: [0028] S11, setting an irregular sea area L (i.e., a task area), which is a convex polygon defined by n.sub.a two-dimensional coordinates meeting a WSG84 standard; [0029] S12, setting n.sub.f irregular restricted area F.sub.1 . . . F.sub.n.sub.f, each no-go zone being a convex polygon defined by

[00003]$n_{a1}\mathrm{.Math..Math.}{n}_{a_{n_f}}$ two-dimensional coordinates meeting a WSG84 standard; [0030] S13, setting parameters and state quantities of n.sub.u unmanned surface vehicles, including a unique identifier id.sub.i, a maximum speed V.sub.i, a turning rate .sub.i, a search radius r.sub.i, whether a task has been assigned b.sub.assign, i, a current position p.sub.i, and a current velocity vector v.sub.i, serving as a structure and stored in the task allocator, and corresponding to connected unmanned surface vehicle real machines one-to-one; [0031] S14, setting parameters and state quantities of m.sub.t targets to be completed, including a unique identifier id.sub.j, whether it has been assigned b.sub.target,j, a work area radius r.sub.j, and a current position q.sub.j, serving as a structure and stored in the task allocator, and corresponding to targets one-to-one; targets can be dynamically added or deleted; [0032] S15, identify unassigned targets T.sub.un and unmanned surface vehicles without assigned tasks U.sub.un through the state quantities of the targets and unmanned surface vehicles, serving as initial inputs for each task assignment.

[0033] Step 2., the task allocator assigns a corresponding target to each unmanned surface vehicle, specifically including: [0034] S21, calculating an adjusted distance d.sub.ij from each target j to each unmanned surface vehicle i, given by the following formula:

[00004]$d_{\mathrm{ij}}^{}=d_{\mathrm{ij}}+d_{\mathrm{ij}}+d_{\mathrm{ij}}$ [0035] wherein, d.sub.ij is the Euclidean distance from the i-th unmanned surface vehicle to the j-th target; d.sub.ij is an adjustment term considering performance parameters of the unmanned surface vehicle for the distance, given by the following formula:

[00005]$d_{\mathrm{ij}}={}_1\frac{.Math.v_i.Math.\cos {}_{\mathrm{ij}}-V_i}{V_i}+{}_2\frac{.Math.{}_i-{}_{\mathrm{ij}}.Math.}{{}_i}$ [0036] wherein, v.sub.i is a velocity vector of the i-th unmanned surface vehicle, .sub.i is a direction of the velocity vector, V.sub.i is a maximum speed, .sub.i is a maximum turning rate, .sub.ij is an azimuth angle from the i-th unmanned surface vehicle pointing to the j-th target, .sub.1, .sub.2 are weighting coefficients, and specific values are set according to different unmanned surface vehicle models, which are set to 0.7, 0.3 respectively in this embodiment; referring to FIG. 2, specific steps for calculating the penalty term d.sub.ij for each target j bypassing restricted area from the unmanned surface vehicle i are as follows: [0037] 1. dividing a straight-line path from the unmanned surface vehicle i to the target j into N segments, each segment having a length of 1, and endpoints of each segment being denoted as sampling points P.sub.0 . . . P.sub.k . . . P.sub.N, with the unmanned surface vehicle as a sampling point P.sub.0; [0038] 2. for values of k from 0 to N1, determining whether a connecting line P.sub.kP.sub.k+1 intersects with a boundary of a no-go zone F.sub.m, and if intersecting, denoting a first intersection point and a last intersection point as a.sub.k, b.sub.k respectively, and deleting sampling points between a.sub.k, b.sub.k; [0039] 3. finding a point n.sub.k on the boundary of F.sub.m that is closest to a.sub.k; [0040] 4. calculating a point a.sub.n,k obtained by extending from n.sub.k outwards along a normal vector direction away from a center point c.sub.m of F.sub.m by a distance d (set according to a range size of the no-go zone); [0041] 5. starting from a.sub.n,k, traversing boundary points of F.sub.m outwards by a distance d in clockwise and counter-clockwise directions to obtain points a.sub.n,k+i, until a connecting line between a.sub.n,k+i and b.sub.k does not intersect with F.sub.m, and recording path lengths in clockwise and counter-clockwise directions L.sub.cw, L.sub.ccw respectively; [0042] 6. selecting a smaller value from L.sub.cw and L.sub.ccw, and denoting it as L.sub.k; [0043] 7. adding path points bypassing F.sub.m, i.e., points in L.sub.k, between the sampling points a.sub.k, b.sub.k, and repeating steps 2. to 6. to process subsequent restricted area F.sub.m+i; [0044] 8. finally obtaining a sampling point sequence P as a path bypassing all F.sub.m, i.e., the sampling point sequence P is composed of N.sub.p waypoints, and a length thereof being d.sub.ij; [0045] 9. for the sampling point sequence P, calculating a turning angle .sub.k between each adjacent sampling line segment P.sub.kP.sub.k+1 and P.sub.k+1P.sub.k+2; [0046] 10. calculating a path turning penalty term:

[00006]$={.Math.}_{k=1}^{N_p-1}{}_k{{}_k}^2,{}_k$ being a turning penalty parameter; [0047] 11. finally, the penalty term d.sub.ij for each target j bypassing restricted area from the unmanned surface vehicle i is a sum of navigation distance and turning penalty: d.sub.ij=d.sub.ij+. [0048] S22, the task allocator assigns a target t.sub.j to each unmanned surface vehicle u.sub.i, wherein the unmanned surface vehicle u.sub.i belongs to unmanned surface vehicles without assigned tasks U.sub.un, and the target t.sub.j belongs to unassigned targets T.sub.un; specifically including: [0049] 1. defining an unmanned surface vehicle set U.sub.un={u.sub.1, u.sub.2, . . . , u.sub.i, . . . , u.sub.m}, and a target set T.sub.u={t.sub.1, t.sub.2, . . . , t.sub.j, . . . t.sub.n}, wherein m and n are respectively a quantity of unmanned surface vehicles to be assigned and a quantity of targets to be assigned, serving as initial inputs for each task assignment; [0050] 2. constructing an adjusted distance matrix D.sub.mn between unmanned surface vehicles and targets by d.sub.ij; [0051] 3. initializing an assignment result of each unmanned surface vehicle u.sub.i to null, i.e., match (u.sub.i)=null; [0052] 4. defining a sub-matrix D, DD.sub.mn, D including rows and columns corresponding to unmanned surface vehicles and targets that have not been assigned yet; [0053] 5. finding a minimum element d.sub.ij in the sub-matrix D, and determining a target t.sub.j assigned to the unmanned surface vehicle u.sub.i by a row and a column of the minimum element; [0054] 6. if the target t.sub.j has not been assigned yet, assigning u.sub.i to t.sub.j, and setting match (u.sub.i)=t.sub.j; [0055] 7. deleting the i-th row and the j-th column from D, indicating that corresponding unmanned surface vehicles and targets have been matched; 8. repeating steps 5. to 7. until all unmanned surface vehicles and targets to be assigned have been matched. [0056] S23, marking unmanned surface vehicles and targets in the matching result match (u.sub.i) as having been assigned, specifically: [0057] 1. for each unmanned surface vehicle u.sub.i in the matching result match (u.sub.i), marking its b.sub.assign, i as True; [0058] 2. for each target t.sub.j in the matching result match (u.sub.i), marking its b.sub.target, j as True;; [0059] removing the unmanned surface vehicles marked as assigned from U.sub.un, and removing the targets marked as assigned from T.sub.un.

[0060] Step 3., a route planner (set on the unmanned surface vehicle) sets waypoints from itself to a target for each unmanned surface vehicle, specifically including: [0061] S31, for the matched unmanned surface vehicles and corresponding targets, referring to the waypoint sequence P.sub.ij bypassing restricted area from the unmanned surface vehicle u.sub.i to the target t.sub.j calculated in S 21; [0062] S32, interpolating P.sub.ij by using a cubic B-spline interpolation algorithm to generate a new waypoint sequence P.sub.ij, specific steps are as follows: [0063] 1. constructing a cubic B-spline curve C(u) by using P.sub.ij as a control point sequence; [0064] 2. defining a turning energy function E.sub.b={umlaut over (C)}.sup.2(u)du, representing turning energy; [0065] 3. defining a route length function E.sub.l=|(u)|du, representing route length; [0066] 4. constructing an objective function E=w.sub.1E.sub.b+w.sub.2E.sub.l, wherein w.sub.1, w.sub.2 are weighting coefficients; [0067] 5. minimizing the objective function by adjusting w.sub.1, w.sub.2 values to obtain a smooth curve C*(u); [0068] 6. sampling the C*(u) curve, with a distance between sampling points being l, to generate a new waypoint sequence P.sub.tij; [0069] 7. deleting points in the new waypoint sequence P.sub.tij that are within restricted area or in a task area to obtain the sequence P.sub.ij. [0070] S33, using the sequence P.sub.ij as a navigation waypoint sequence from the unmanned surface vehicle u.sub.i to the target t.sub.j, and sending and storing it in a navigation system of a corresponding unmanned surface vehicle.

[0071] Step 4., a navigation controller on the unmanned surface vehicle real machine continuously adjusts a navigation attitude of the unmanned surface vehicle in navigation to meet cooperative obstacle avoidance and sea area constraint requirements, specifically including: [0072] S41, in each time step dt of solving by the optimizer, for each unmanned surface vehicle u.sub.i, checking whether there are other unmanned surface vehicles u.sub.j(ij) or targets t.sub.k within a search radius r.sub.i, and if yes, adding the other unmanned surface vehicles u.sub.j(ij) or targets t.sub.k to an obstacle set Obs.sub.i; [0073] calculating an expected velocity vector v.sub.des of the unmanned surface vehicle u.sub.i to its next waypoint P.sub.iP.sub.ij, wherein a direction is a unit vector from u.sub.i to P.sub.i, and a magnitude is a maximum speed V.sub.i; [0074] S42, inputting parameters and state quantities of the unmanned surface vehicle u.sub.i, the obstacle set Obs.sub.i, the restricted area F.sub.m, the task area L, and the expected velocity vector v.sub.des into the optimizer to perform a solving operation, specific steps are as follows: [0075] 1. the optimizer constructs an objective function:

[00007]$J\left(v\right)={.Math.v_{\mathrm{des}}-v.Math.}^2+c_{\mathrm{penalty}}\left(v\right)$ [0076] wherein, v is an optimization variable, representing a velocity control vector of the unmanned surface vehicle; c.sub.penalty is a penalty term to prevent the unmanned surface vehicle from running still after falling into a local optimum, when v is less than max(0.8*v.sub.des, 0.8*V.sub.i), c.sub.penalty is 100, otherwise it is 0. [0077] 2. the optimizer constructs constraint conditions: [0078] c.sub.1: |v|V.sub.i, ensuring that a speed is less than a maximum speed;
c.sub.2: p.sub.next.Math.F.sub.m, ensuring that a future position is not within restricted area; [0079] c.sub.3: p.sub.nextL, ensuring that a future position is within a task area. [0080] wherein, p.sub.next=P.sub.i+v.Math.dt is a next moment position calculated according to a velocity control quantity v. [0081] 3. using an SLSQP algorithm to solve for a velocity control quantity v* that minimizes the objective function J(v):

[00008]$\underset{v}{\min }J\left(v\right)s.t.c_1,c_2,c_3$ [0082] outputting the solved v* as the velocity control quantity. [0083] S43, using the velocity control quantity v* to drive the unmanned surface vehicle u.sub.i to travel towards the target waypoint P.sub.i, and after reaching the waypoint, deleting P.sub.i from the waypoint sequence P.sub.ij until reaching a last waypoint in the waypoint sequence P.sub.ij, i.e., reaching a task area of the target t.sub.j, and after performing a task, adding the unmanned surface vehicle u.sub.i to U.sub.un again, and restarting step 2, until all tasks are completed.

[0084] FIG. 3 is a diagram of a real machine operation interface of a method for collaborative target assignment and route planning for multi-agent unmanned surface vehicles according to the present application. It should first be clarified that FIG. 3 is only present as an exemplary diagram and cannot limit the scope of the present application.

[0085] The above embodiments are used solely to illustrate the technical solution of the present application and are not intended to limit it. Although the present application has been described in detail with reference to the aforementioned embodiments, those of ordinary skill in the art should understand that they can still modify the technical solutions described in the aforementioned embodiments or replace some of the technical features with equivalent ones. Such modifications or replacements do not cause the essence of the corresponding technical solutions to depart from the spirit and scope of the technical solutions of the embodiments of the present application, and should all be included within the scope of protection of the present application.