Task Assignment and Path Planning for Multiple Autonomous Underwater Vehicles Using 3D Dubins Curves †

This paper investigates the task assignment and path planning problem for multiple AUVs in three dimensional (3D) underwater wireless sensor networks where nonholonomic motion constraints of underwater AUVs in 3D space are considered. The multi-target task assignment and path planning problem is modeled by the Multiple Traveling Sales Person (MTSP) problem and the Genetic Algorithm (GA) is used to solve the MTSP problem with Euclidean distance as the cost function and the Tour Hop Balance (THB) or Tour Length Balance (TLB) constraints as the stop criterion. The resulting tour sequences are mapped to 2D Dubins curves in the X−Y plane, and then interpolated linearly to obtain the Z coordinates. We demonstrate that the linear interpolation fails to achieve G1 continuity in the 3D Dubins path for multiple targets. Therefore, the interpolated 3D Dubins curves are checked against the AUV dynamics constraint and the ones satisfying the constraint are accepted to finalize the 3D Dubins curve selection. Simulation results demonstrate that the integration of the 3D Dubins curve with the MTSP model is successful and effective for solving the 3D target assignment and path planning problem.

Assume a set of static targets T = {T 1 , T 2 , ..., T N } and a set of homogeneous mobile AUVs A = {A 1 , A 2 , ..., A K } that are randomly deployed in the X × Y × Z three dimensional underwater space, with N and K denoting the total numbers of static targets and mobile AUVs, respectively. Also assume K < N, as this is commonly encountered in many practical applications. Each AUV has the same initial energy E init and the same energy consumption model which is a linear function of its tour length. In order to illustrate design detailed methodology of proposed algorithm, we summarize the simplified notation in Table 1 for the reader's convenience.

Notation Definition
T the set of static targets A the set of mobile AUVs T n the n-th static target A k the k-th mobile AUV N total number of static targets K total number of mobile AUVs D k tour sequence for the kth AUV, k = 1, 2, ..., K S k tour trajectory for the kth AUV, k = 1, 2, ..., K N k the number of targets in sequence D k µ(N k ) intra-AUV mean of the number of assigned targets L k tour length of sequence D k µ(L k ) intra-AUV mean of the assigned tour lengths The objective of task assignment and path planning is to assign a tour sequence of targets D k , k = 1, 2, ..., K from the target set T to each AUV such that each target is visited by an AUV once and only once, and the total cost of visiting all targets is minimized.
Let N k and L k denote the number of targets and tour length of sequence D k , respectively. The tour cost of tour sequence D k is denoted as C(D k ), and the task assignment and path planning problem is to minimize D k D l = ∅, ∀k, l and k = l Var(N k ) → 0 (4) where Equations (4) and (5) are the Tour Hop Balance (THB) constraint and the Tour Length Balance (TLB) constraint, respectively, and Var(·) denotes the intra-AUV variance which is calculated in the following section, → means as small as possible. For example, N k = N K , ∀k if N is divisible by K.

Vehicle Kinematic Constraints
An AUV belongs to a body-fixed coordinate system with six degrees of freedom, and so its motion can be described relative to an inertial-fixed reference frame. However, we only consider the position value and motion heading of an AUV in this paper since it is enough for path planning with Z-axis linear interpolation method. The location and motion of an AUV in three-dimensional Cartesian space are shown in Figure 2, where the position of the AUV is denoted as X = {x, y, z}, and its motion heading is denoted as Φ = {φ, θ}, where θ and φ are the X-Y plane angle and Z-axis angle projected from the AUV's motion heading, respectively. The velocity scalar of the AUV is denoted as F, and the projected X-Y plane angle and Z-axis angle are bounded, making its motion nonholonomic constraints [8] such that where the dot operator is the derivative, anḋ where ω 1 and ω 2 represent the curvature bounds. The nonholonomic constraints requires that the AUV paths satisfy the G 0 and G 1 continuities [24,25] which are defined as follows: G 0 continuity: P(u) = [x(u), y(u), z(u)] and Q(w) = [x(w), y(w), z(w)] be two regular parametric 3D curves in the X × Y × Z space, where u ∈ [0, 1] and w ∈ [0, 1] are the parameters with 0 and 1 denoting the starting and ending points, respectively. If P(1) = Q(0) = J, then the two parametric curves meet at joint J with G 0 continuity. G 1 continuity: IfṖ(u)| u=1 =Q(w)| w=0 , then the two parametric curves meet at joint J with G 1 continuity.

The 2D Dubins Curve
For 2D path planning kinematic constraints, a classical path model is to use the 2D Dubins curve [6,7] to satisfy the G 1 continuity. Given any two points in the X × Y plane, starting at x 0 = (x 0 , y 0 ) and ending at x 1 = (x 1 , y 1 ), the Dubins curves satisfy the dynamic constraints expressed in Equations (6) and (7) by a combination of maximum curvature arcs (C) and/or a straight line segment (S) to form two families of curves: family CCC and family CSC. Note that all arcs are with radius ρ. The family CCC contains curves with types RLR and LRL, where R and L denote a right turn arc (or clockwise) and a left turn arc (counter clock), respectively; The family CSC includes four types: RSR, LSL, RSL, and LSR, where S is a straight line segment. Therefore, the shortest Dubins path between two points are selected from the six types = {LSL, RSR, RSL, LSR, RLR, LRL}.
For example, Figure 3 shows the four CSC types of Dubins curves with φ 0 = −π/4 and φ 1 = −3π/4 and two points (0, 0) and (0, −d). Note that φ 0 and φ 1 are measured counter-clockwise with respect to the positive x-axis. It is obvious that the Type 1 Dubins curve has the shortest length. The 2D Dubins curves have been well investigated in the literature. It has been shown [7] that for the long path case where the distance between the starting point and ending point, denoted as d and normalized by the turning radius ρ, satisfies d > 4 − (| cos φ 0 | + | cos φ 1 |) 2 + | sin φ 0 | + | sin φ 1 |, the shortest path cannot be in the CCC family. The shortest paths with given φ 0 and φ 1 can be easily determined by the quadrants that the two angles fall in Ref. [7]. The elements of the 4 × 4 matrix, a i,j , represents the quadrant number i of the starting angle and the quadrant number j of the ending angle. The shortest 2D Dubins curves starting from (0, 0) and ending at (d, 0) are then determined by Table 2, where the different types in Table 2 are determined by certain switching functions defined in [7]. The exact path and its length can be calculated by the three operators, L γ for left turn, R γ for right turn, and S γ for straight, which transform an arbitrary point [x, φ] into its corresponding image point where x = (x, y), and the index γ means that the path segment is of length γ. For example, the LSR path with respective lengths of t, p, q between point or The solution is denoted t LSR , p LSR , and q LSR , respectively. The total path length is then L LSR = t LSR + p LSR + q LSR = φ 1 − φ 2 + 2t LSR + p LSR . Details of other types of paths can be found in [7].

The Multiple Traveling Salesmen Problem for Dubins Vehicle
Multi-vehicle path planning is often casted as a Multiple Traveling Salesmen Problem (MTSP) [15] which is to find a set of closed paths for multiple traveling salesmen to visit a set of cities such that each and every city is visited exactly once and the total cost of visiting all cities are minimized. In the AUV path planning problem, the cost is the sum of Euclidean distances along the paths. It is difficult to find the optimal solution to the MTSP and heuristic iterative algorithms, such as the Genetic Algorithm (GA), Reactive Tabu Search, and Clustering and Actioning [8] are often used. In this work, we use the GA algorithm [15].
The MTSP is first converted into an equivalent mixed integer programming (MIP) problem [15] by introducing a binary selection variable I k i,j ∈ {0, 1} defined as the indicator for the kth iteration of GA algorithm. If I k i,j = 1, then the AUV A k is assigned to travel from target T i to target T j . Otherwise, if I k i,j = 0, then AUV A k is assigned not to visit targets T i and T j . Given an undirected graph G = (T , E ), where T is the set of static targets (nodes), E = {(i, j), i, j ∈ T } is the set of edges, and i, j = 0, 1, · · · , N with i = 0 and j = 0 denoting the home node or the starting/returning location The constraints (13) and (14) ensure that the K salesmen start from the home node and return to the home node. Constraints (15) and (16) ensure that each node is visited (entered and left) only once. The constraint (17) is to ensure that the cost of each AUV is capped at S. Note that the constraints in (4) and (5) are used as the stop criterion.
The GA algorithm solves the MTSP by treating all possible tour sequences as the population, a specific tour sequence as an individual, the nodes in a tour sequence as a chromosome, and the travel length of a tour sequence as the fitness function. The GA algorithm starts with a random population with M individuals, and calculates the fitness function for each of the M individuals; then it creates new population by parent selection, parent crossover, chromosome mutation, and descendant acceptance; A new population results from replacing individuals by descendants with better fitness. Next the generated new population is used in the next iteration that iterates through the new population generation process, until the stop condition is satisfied. The solution to the MTSP is the set of selection variables I k i,j . The tour sequence for the kth AUV is the set of edges selected by the GA algorithm with Note that the AUV dynamics and G 1 continuity constraints have to be considered when applying the MTSP model to AUVs target assignment and path planning. Using Dubins curves, the MTSP model can be applied to solve the Dubins target assignment and path planning problem in three steps:

•
Step 1 uses the Euclidean distances between the nodes as the cost c i,j and assigns the N targets to the K AUVs through the GA algorithm.

•
Step 2 converts the tour sequence of each AUV into the Dubins paths by selecting a set of headings at each node and computing the lengths of Dubins curves; • Step 3 chooses the Dubins path and its associated headings with the shortest total distance.
The headings of the AUVs are discretized to 2 B angles such that φ 0 , φ 1 , θ 0 and θ 1 take values at πb/2 B with b = 1, · · · , 2 B+1 − 1 and excluding multiples of π/2. The discretization can greatly reduce the computational complexity in searching for the shortest Dubins path. The total length of a tour sequence is then calculated as for k = 1, · · · , K, and the total cost of all AUVs is computed as in (1). In comparison, an existing method called the Alternating Algorithm [23] also uses the first approach that solves the Euclidean MTSP then maps the tour sequences to Dubins path. However, to reduce the computational complexity of the Dubins search, only odd-indexed edges in the tour sequence are replaced by minimum length Dubins paths, the even-indexed edges keep the straight Euclidean path.

Target Assignment and Path Planning in 3D Space
This section extends the target assignment and path planning algorithms from 2D to 3D by incorporating the 3D Dubins curves. We use the first approach in which the GA algorithm solves the Euclidean MTSP for the multiple targets and multiple AUVs, then maps the Dubins curves in 3D. We follow the simple linear interpolation method in [13,14] and analyze the G 1 continuity of the resulting 3D Dubins paths.

The 3D Dubins Curves and Their Path Lengths
To extend the 2D Dubins curves to 3D space using the linear interpolation method, the 3D tour sequences are first projected on to the 2D X × Y plane in a global coordinate system. Taking a starting point [X 0 , Φ 0 ] and an ending point [X 1 , Φ 1 ] in the 3D space and project them on to the 2D plane, as shown in Figure 4. Then the starting and ending points become 2D parameters [(x 0 , y 0 ), φ 0 ] and [(x 1 , y 1 ), φ 1 ]. The 2D Dubins curve is designed as described in Section 2.2, and the lengths of the arcs and line segment are calculated by (10). Let L 0,x and L 0,1 denote the lengths along the 2D Dubins curve from (x 0 , y 0 ) to (x, y) and from (x, y) to (x 1 , y 1 ), respectively.
The linear interpolation adds the z coordinate by where z 0 and z 1 are the Z coordinates of the starting and ending points. The resulting 3D Dubins curve is illustrated in Figure 4a. The length of the interpolated 3D Dubins curve is calculated as where t, p, and q are the CSC segment lengths of the 2D Dubins curve, and z m and z n are the Z coordinates of the segment joints which are linearly interpolated as  Equation (21) is easily derived from Figure 5, since for the straight segment, the sides with length p, p * , and z m − z n form a right triangle. Thus, p * = p 2 + (z m − z n ) 2 . For the left turn segment (the ending segment in this example), q * = q 2 + (z 1 − z n ) 2 if the cylinder containing the arc segments q * and q is opened and laid flat, thus the segments q, q * and z m − z 1 form a right triangle. Similar to the left turn segment, the right turn segment satisfies t * = t 2 + (z m − z 0 ) 2 . As a result, the total length of the 3D Dubins curve is L 3D = t * + p * + q * . Proof. Let L 2D = t + p + q be the length of the 2D Dubins curve. Substituting (22) into (21) yields Therefore, the shortest length L 2D of 2D Dubins curve leads to the shortest 3D Dubins curve. Theorem 2. The 3D Dubins curves generated by linear interpolation of Z coordinates from the 2D Dubins curve can preserve G 1 continuity in the X − Y plane but would lose the G 1 continuity in Z. Figure 4, the 2D Dubins curve between a starting and an ending target is composed of three segments: arc, line, and arc, which joint at two joints. These three segments meet at the joints with G 1 continuity because the line segment designed by (10) ensures the G 1 continuity of each Dubins curve on the X − Y plane. When the tour sequence contains multiple targets, the two Dubins curves meet at one target location, and the G 1 continuity can be preserved by forcing the end heading φ 1 of the first Dubins curve equal to the start heading φ 0 of the second Durbins curve.

Proof. As shown in
However, in the Z domain, the linear interpolation among three or more targets cannot guarantee the G 1 continuity at the joints. For example, let targets 1, 2, and 3 be linked by two line segments. The end of the first line segment joins the start of the second line segment. when the two line segments are not aligned, the start heading θ 0 of the second line segment (in Z domain) is not equal to the end heading θ 1 of the first line segment. Therefore, the two line segments meet at the joint without G 1 continuity. For a particular example: z i−1 = 10, z i = 15, z i+1 = 10, so the AUV will have to increase its height from X(i − 1) to X(i), and it must decrease its height from X(i) to X(i + 1) . If the AUV has an initial upward heading angle φ i−1 because z i − z i−1 = 5, then it might turn to a downward heading φ i at X(i) because z i+1 − z i = −5. Therefore, the AUV will have to change its heading angle Φ = (φ, θ) suddenly before traveling down to z i+1 = 10.

Path Planning for Multiple AUVs in 3D Space
This section describes the detailed solution to the multiple targets tracking task assignment problem in three dimensional underwater workspace with constraints of Tour Hop Balance (THB) or Tour Length Balance (TLB). The three step approach in Section 2.3 is used, where step 1 applies the multiple traveling salesman problem (MTSP) algorithm with Euclidean distances as the fitness function.
To incorporate the Tour Hop Balance (THB) or Tour Length Balance (TLB) constraints into the Genetic Algorithm, the variances of N k and L k are estimated aŝ whereμ(N k ) andμ(L k ) are the estimated expectations of N k and L k , respectively. These variances are used by the GA as the termination rule. The resulting algorithms are called the THB-3Dubins-MTSP algorithm and TLB-3Dubins-MTSP algorithm, respectively. The outputs of the GA algorithms are a set of tour sequences for all AUVs. Then step 2 maps the direct paths in a tour sequence into 2D Dubins curves, which is accomplished by projecting the 3D target locations onto the X − Y plane and design 2D Dubins curves. Since different headings on the 2D plane can result in different solutions, the curves include all possible starting φ 0 and ending φ 1 for all target pairs. The shortest 2D path is selected by computing the total distances and selecting the smallest one.
The last step converts the 2D Dubins path into 3D curves by linear interpolation of the Z coordinates at each target pair. Then the headings θ 0 and θ 1 are calculated. Since linear interpolation loses the G 1 continuity, the difference between θ 0 and θ 1 at each joint J of the 3D Dubins path is computed as Θ(J) = θ 1 (J) − θ 0 (J). The results Θ(J) for all J are compared against the bound ω 2 . If any joint of the Dubins path has a Θ(J) > ω 2 , then the 3D Dubins path is rejected, and another 2D Dubins path, computed in Step 2, has to be used to be interpolated to the 3D Dubins curve. The process is repeated until the 3D Dubins path satisfies the vehicle dynamics constraint. This method is called rejection-acceptance method.
In summary, the pseudo-code of the proposed algorithms is described in Algorithm 1. For all we know, there are a small collision probabilities when multiple robots cruising in the same 3D underwater space. Two and more than two AUVs will be collided when multiple AUVs are arriving at the same coordinates at the same time. However, we run simulations of MTSP for collision check and find the collision between AUVs is negligible since the quantity of AUVs is small because of its high cost. Moreover, another sub-optimal paths can be generated with GA if there is a collision.

Simulation Results
Simulations were set up with multiple underwater targets deployed randomly in a cube of 20 × 20 × 10 units, where 1 unit is the minimum turning radius ρ of each AUV. For example, the turning radius of Iver2 AUV used in [8] is set to 5 m, so the proposed unit equals to 5 m. The number of targets varied from 10 to 50 in increments of 5, and the number of AUVs varied between 2 and 5. For the sake of simplicity, we choose the typical azimuth headings set for AUV movement as φ ij ∈ { bπ 4 } with b = 1, 3, 5, 7. The proposed algorithm was implemented in Matlab and was developed on an Intel 2.5GHz i5-3210M CPU with 4GB RAM and running Windows 7. The computing time of the proposed algorithm with different targets number and AUV number are compared in Figure 6. The parameters for the GA is listed in Table 3.   Figure 7 illustrates 3D Dubins based MTSP trajectories of 2 or 4 AUVs generated by 3Dubins-MTSP algorithm. The x-axis and y-axis coordinates with their differential values are compared in Figure 8. In comparison, the 3D Alternating Algorithm (AA-3DTSP) extended the 2D Alternating Algorithm [23] by assigning Z-coordinates with linear interpolation and the resulting path is shown in Figure 9. A DTSP tour in (AA-3DTSP) can be constructed by retaining all odd-numbered edges (except the n-th), and replacing all even-numbered edges with minimum-length Dubins' paths preserving the point ordering. In Figure 9, the Green curves denote odd-numbered edges, and the Blue curves denote even-numbered edges. It is clear non-smooth trajectories fail to G 1 continuity in either the X-Y plane nor the z-axis, as shown in Figure 10. Comparing Figure 10 to Figure 8, it is evident that cruise trajectories derived from our algorithm are G 1 continuous, however, cruise trajectories derived from Alternating Algorithm are only G 0 continuous because the tangent angle of each point on the path is not continuous. Therefore, 3D Alternating Algorithm (AA-3DTSP) is not appropriate for nonholonomic AUV and so it is excluded in the following simulations.
Next, we demonstrate the effectiveness of the THB-3Dubins-MTSP algorithm and TLB-3Dubins-MTSP constraints in comparison with the TSP without constraints and the Random Tour (RT) Algorithm. The Random Tour (RT) algorithm uses a set of random headings to achieve cruise paths without any constraints. The 3D Dubins based TSP (3Dubins-TSP) algorithm use only one AUV to trace all targets while cruising along 3D Dubins curves. Performance metrics include energy consumption, energy balance rate, cruise speed, and task life cycle. Energy consumption denotes the total energy consumption of all AUVs in the mission of tracking all targets, which is measured by the total tour length with assumption that each AUV consumes one unit energy at each unit tour length. Energy balance rate denotes with RMS (root mean square) value of energy consumption of each AUV, which is defined as Equation (28). Cruise speed is defined the maximal tour length of the AUV team in one cruise process, which denotes that the maximal time needed to finish one cruise for all targets. Task life cycle denotes the repeated number of each cruise mission, and it equals to the round number when one of AUV exhausts its energy. In this paper, it is assumed that the mobile AUV will carry out targets detection mission iteratively, and cruise lifetime is measured as the number of targets detection mission.
For a given number of targets, we simulated 100 Monte Carlo trails and computed the average length of 3D tours generated by different algorithms. The standard deviation value is relatively small since the proposed algorithms can achieve progressive optimization using a large number of iterations of GA. The comparison results of energy consumption, energy balance, cruise speed and task life cycle are shown in Figures 11-14, respectively.     Figure 11 shows that our proposed algorithms consume almost the same energy comparing to Random Tour algorithm, but they are much lower than 3Dubins-TSP algorithm. Figure 12 shows the improvements of energy balance ratio, there is at most 50% improvement with Random Tour algorithm on the RMS metric. Figure 13 shows the cruise speed comparisons of different mechanism, we find the cruise distance (e.g., the maximal cruise time) will decrease with the proposed algorithms clearly, and it is even obvious with more AUVs. Figure 14 shows the task life cycle comparisons, it is obvious the lifetime can be extended with our proposed algorithms, especially with more targets in the underwater region. In summary, the proposed THB-3Dubins-MTSP and TLB-3Dubins-MTSP algorithms will improve performances such as energy balance ratio, cruise speed and task life cycle comparing to Random Tour (RT) algorithm greatly, thus verify that the proposed algorithms can achieve better performance with G 1 continuous constraints. Moreover, the proposed THB-3Dubins-MTSP and TLB-3Dubins-MTSP algorithms have similar performances.

Conclusions and Future Work
This paper has studied the 3D Dubins curves for target assignment and path planning for multiple underwater targets visited by multiple AUVs. The MTSP for 3D path planning is solved by using the inverse of the Euclidean distances as the fitness function and the Tour Hop Balance (THB) and Tour Length Balance (TLB) constraints as the stop criterion. The resulting target assignment (tour sequence) is then projected onto the 2D X − Y planes and 2D Dubins curves are designed with a set of possible headings and with nonholonomic motion constraints. The resulting 2D Dubins curves are interpolated linearly to obtain the Z-coordinates of each 2D curve. We derived the path length calculation for 3D Dubins curves and analyzed the G 1 continuity of the 3D Dubins curve. It is demonstrated that the linear interpolation fails to achieve G 1 continuity in the Z coordinate, and other smooth interpolation may have to be used when the continuity is required. Moreover, we find there are small collision probabilities between AUVs from above analysis. Therefore, in our future work, we will study how to avoid multiple underwater obstacles and inter-AUV collision for path planning of multi-AUV team.