Method for target assignment and route planning for multi-agent unmanned surface vehicles

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 Tun and a set of unmanned surface vehicles without assigned tasks Uun 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
• ij
• ′
• from each target j to each unmanned surface vehicle i:
• d
• ij
• ′
• =
• d
• ij
• +
• Δ
• ⁢
• d
• ij
• +
• Ω
• ⁢
• d
• ij
• wherein, dij is the Euclidean distance from the i-th unmanned surface vehicle to the j-th target, Δdij is an adjustment term considering performance parameters of the unmanned surface vehicle for the distance, and Ωdij is a penalty term for the j-th target bypassing restricted area from the i-th unmanned surface vehicle;
• constructing an adjusted distance matrix Dm×n between unmanned surface vehicles and targets by
• d
• ij
• ′
• ,
• defining a sub-matrix D′, D′∈Dm×nD′ comprising rows and columns corresponding to unassigned unmanned surface vehicles ui and targets tj, and matching the unassigned unmanned surface vehicles ui and targets tj based on a minimum element in D′;
• wherein the unmanned surface vehicle ui belongs to the set of unmanned surface vehicles without assigned tasks Uun, and the target tj belongs to the set of unassigned targets Tun;
• 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 P0 . . . Pk . . . PN;
• 2) Determining whether a connecting line PkPk+1 intersects with a boundary of a restricted area Fm, and if intersecting, denoting a first intersection point and a last intersection point as ak, bk respectively, and deleting sampling points between ak, bk;
• 3) Finding a point nk on the boundary of Fm that is closest to ak;
• 4) Calculating a point an,k obtained by extending from nk outwards along a normal vector direction away from a center point cm of Fm by a distance d;
• 5) Starting from an,k, traversing boundary points of Fm outwards by a distance d in clockwise and counter-clockwise directions respectively to obtain points an,k+i, until a connecting line between an,k+i and bk does not intersect with Fm, and recording path lengths in clockwise and counter-clockwise directions Lcw, Lccw respectively;
• 6) Selecting a smaller value from Lcw and Lccw, and denoting it as Lk;
• 7) Adding path points bypassing Fm, i.e., points in Lk, between the sampling points ak, bk, and repeating the steps 2 to 6 to process subsequent restricted area Fm+i;
• 8) Finally obtaining a sampling point sequence P as a path bypassing all Fm, and a length thereof being
• Ω
• ⁢
• d
• ij
• ′
• ;
• 9) For the sampling point sequence P, calculating a turning angle θk between each adjacent sampling line segment PkPk+1 and Pk+1Pk+2;
• 10) calculating a path turning penalty term:
• Ωω
• =
• ∑
• k
• =
• 1
• N
• p
• -
• 1
• ⁢
• ω
• k
• ⁢
• θ
• k
• 2
• ,
• ω
• k
• being a turning penalty parameter;
• and
• 11) The penalty term Ωdij for each target j bypassing restricted area from the unmanned surface vehicle i is:
• Ωdij=Ωdij′+Ωω;
• for the matched unmanned surface vehicles and corresponding targets, calculating to obtain a waypoint sequence Pij bypassing restricted area from the unmanned surface vehicle ui to the target tj, and interpolating Pij to generate a new waypoint sequence P′ij;
• using the sequence P′ij as a navigation waypoint sequence from the unmanned surface vehicle ui to the target t, 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 ui, an obstacle set Obsi, restricted area Fm, a task area L, and an expected velocity vector vdes of the unmanned surface vehicle ui to its next waypoint Pi∈P′ij into an optimizer, and solving to obtain a velocity control quantity;
• and
• using the velocity control quantity to drive the unmanned surface vehicle ui to travel towards the target waypoint Pi, and after reaching the waypoint, deleting Pi from the waypoint sequence
• P
• ij
• ′
• until reaching a fast waypoint in the waypoint sequence
• P
• ij
• ′
• ,
• i.e., reaching a task area of the target tj, and after performing a task, adding the unmanned surface vehicle ui to Uun again, and restarting step 2, until all tasks are completed.