Dynamic positioning of beacon vehicles for cooperative underwater navigation

Autonomous Underwater Vehicles (AUVs) are used for an ever increasing range of applications due to the maturing of the technology. Due to the absence of the GPS signal underwater, the correct estimation of its position is a challenge for submerged vehicles. One promising strategy to mitigate this problem is to use a group of AUVs where one or more assume the role of a beacon vehicle which has a very accurate position estimate due to an expensive navigation suite or frequent surfacings. These beacon vehicles broadcast their position and the remaining survey vehicles can use this position information and intra-vehicle ranges to update their position estimate. The effectiveness of this approach strongly depends on the geometry between the beacon vehicles and the survey vehicles. The trajectories of the beacon vehicles should thus be planned with the goal to minimize the position uncertainty of the survey vehicles. We propose a distributed algorithm which dynamically computes the locally optimal position for a beacon vehicle using only information obtained from broadcast communication of the survey vehicles. It does not need prior information about the survey vehicles' trajectory and can be used for any group size of beacon and survey vehicles.


I. Introduction
Autonomous Underwater Vehicles (AUVs) navigating underwater face significant localization challenges when compared to aerial or ground vehicles. High frequency electromagnetic waves such as Global Positioning System (GPS) signals do not penetrate the water more than a few millimeters. For the same reason other localization methods based on optical systems such as laser range finders or cameras cannot be employed except in a few niche applications. AUVs thus rely mostly on deadreckoning navigation using proprioceptive sensors which inadvertently accrue a position drift which grows without bound. The most common methods which provide The work presented in this paper was carried out at MIT and EPFL. At MIT it was funded by Office of Naval Research grants N00014-97-1-0202, N00014-05-1-0255, N00014-02-C-0210 and N00014-07-1-1102. At EPFL it was funded by the Spin Due to the limited range of these systems (< 10 km) and the time required to deploy them they are normally used in applications where the are of operations is only a few km 2 . In addition to the restrictions submerged vehicles face with respect to navigation, communication is also restricted to slow (≈ 1 kBytes/s), acoustic channels, where the limited bandwidth only allows for only one vehicle transmitting at a time.
As AUV technology matures AUV operations shift from deploying a single vehicle to deploying a larger group of vehicles. Deploying several AUVs is beneficial for many applications, particularly those which require a large area to be surveyed or redundancy is key. In addition to these benefits a group of AUVs provide the opportunity to implement Cooperative Navigation (CN). The underlying idea of CN is that in group of vehicles where the individual members operate sufficiently close together the individual vehicles can exchange navigation information and thereby improve their own position estimate. This requires that each vehicle is outfitted with an acoustic modem for vehicle-to-vehicle communication.
In addition the modem provides intra-vehicle ranges through one-way or two-way travel time measurements. If in such a group a single vehicle has a more accurate position estimate than the other members, the position estimate of all vehicles within communication range can be improved.
One possible CN strategy has no vehicles which are designated navigation aids and the approach simply relies on the fact that some vehicles may have a better navigation estimate than others. As all vehicles usually broadcast their position estimate to provide a telemetry feedback to the surface operator this approach does not require any additional hardware or communication bandwidth, but only software which incorporates the overheard telemetry packages.
For applications however, such as precision surveys, where navigation accuracy is key the concept of designated Communication and Navigation Aids. The concept of dedicated CNAs was first proposed in [1] for a mine-hunting scenario with the underlying idea that a very small number of CNA (one or two) with a very accurate estimate of their positions could be used to provide a much larger group of Search, Classify and Map SCM-vehicles with navigation information. These SCM-vehicles would be equipped with a special sonar payload to detect buried or free-floating mines. The CNA would be either surface crafts with a permanent access to GPS or an AUV with a very accurate (and expensive) navigation suite. To maintain a bounded uncertainty on their position estimates, these CNA would move at a very shallow depth and surface for a GPS fix whenever necessary. The SCM outfitted with much simpler (and cheaper) navigation sensors would be able to maintain a bounded uncertainty on their position estimates without surfacing over the entire duration of the mission.
The sole mission objective of the CNA is to minimize the overall uncertainty of the SCM vehicles. To accomplish this, their first objective is to maintain a very good estimate about their own position, as in CN any uncertainty in the CNA's position directly translates into an uncertainty in the SCM's position. In addition, the relative position between the CNA and SCM will also strongly affect the position uncertainty of the SCM as shown in [2]. Therefore the second objective of the CNA is to adjust its position such that the CNA-SCM geometry is optimal for CN. This paper proposes an algorithm which attempts to determine a locally optimal position of the CNAs based only on the information available to them through the CNAs' sensors and broadcast messages from the SCMs. No a priori knowledge of the SCMs' trajectory is required. It also does not need any information about the number of CNAs and SCMs. As a result CNA or SCM vehicles which are temporarily outside the communication range will automatically be removed from the optimization and added back once communication is reestablished. This property makes our approach robust against the strong variations of the acoustic communication channel. If additional information is available however several algorithms such as the one proposed in [3] and [4] exist which can provide a globally optimal trajectory.

II. Related work
One particular strength of our method is that it does not need the path of the AUVs to be known a priori. If however such information is available, the approach presented by Chitre [3] and Tan [4] can provide more optimized trajectories.
The problem of selecting an action for an agent, in our case the speed and course of our CNA, in a situation in which several objectives have to be satisfied has been the subject of extensive research [5] and [6]. These methods typically switch between satisfying the different goals individually or perform averaging which does not necessarily lead to the optimal solution. Dias et al. provide a good survey about market-based methods [7].
Benjamin developed the IVP-method which can compute an optimal solution for a set of piece-wise linear objective functions [8]. This implementation was tested in several different scenarios and has demonstrated an Autonomous Surface Craft successfully reaching a waypoint while observing the "rules of the road" [9] and tracking Fig. 1. A set of 5 vehicles performing CN using an EKF. Each vehicle i maintains the distribution over its state (red) through a mean vector µ i and the associated covariance matrix P i . This information, along with the unique ID is broadcasted to other vehicles. Here a broadcast from vehicle 2 is received by vehicle 1.
underwater targets with a towed array while ensuring that its maneuvers would not damage the array [10].
While not addressing the problem of dynamically positioning the beacon, several papers deal with the specific case of one or more submerged vehicles navigating of a single beacon. They also address the influence intravehicle geometry on the effectiveness of the approach [11], [12].

III. Cooperative Navigation
When applying the EKF to solve the problem of CN, we assume that all n vehicles of the set of participating vehicles V i = {1, . . . , i, . . . , n} maintain a vector which consists of the mean vector that contains the estimate of their position at time k, as well as P i the covariance matrix describing the uncertainty associated with that estimate.

A. Prediction
Whenever vehicle i = 1 obtains proprioceptive measurements u 1 (k) from its dead-reckoning sensors, µ 1 (k) and P 1 (k) are propagated (µ and P denote the state after the predict step, but before the update step) where Q 1 (k + 1) is a matrix where the elements contain the variances of the motion model which is modeled as zero-free Gaussian noise and G 1 (k + 1) is the Jacobian containing the partial derivatives of g.

B. Update
If vehicle 1 receives a broadcast from vehicle 2 at k that contains µ 2 (l) and P 2 (l) together with an intra-vehicle range measurement r 1,2 (k), it uses this information to update its estimate of its own position as follows: First, it computes what the predicted range z 1,2 (k) between the two vehicles would be, based on their estimated position.
The difference between the predicted measurement and the measured distance z 1,2 (k) − r 1,2 (k) represents the innovation.
The covariance matrix of vehicle 1 and vehicle 2 are combined into Note that P 1 (k + 1) and P 2 (k + 1) are assumed to be independent (P 1,2 (k + 1) is diagonal). This is not generally true and if the non-zero off-diagonal elements of P 1,2 (k + 1) are ignored, the EKF can become overconfident and diverge. As keeping track of these elements in CN is very difficult, however, [13] proposes a method which keeps P 1 (k + 1) and P 2 (k + 1) independent. We compute the Jacobian H 1,2 (k + 1) that contains the derivatives of the range measurement with respect to the position of vehicle 1 and 2 (time index k omitted on matrix components). Using the residual covariance and the variance S 1,2 (k + 1) = H 1,2 (k + 1)P 1,2 (k + 1)H T 1,2 (k + 1) + σ 2 r and σ r associated with the exteroceptive (range) sensor we compute the Kalman gain that represents a weighting factor for how much the measurement will affect the updated position. Using the innovation z 1,2 (k) − r 1,2 (k) and the Kalman gain, the updated combined position estimate is µ 1,2 (k + 1) = µ 1 (k + 1), µ 2 (k + 1) = µ 1,2 (k + 1) + K 1,2 (k + 1) z 1,2 (k) − r 1,2 (k) (5) and the combined covariance is from which we can extract the updated position estimate µ 1 (k + 1) and the updated covariance P 1 (k + 1) for vehicle 1. Note that we also obtain an updated estimate for the position and covariance of vehicle 2 P 2 (k +1) and µ 2 (k + 1).

IV. Algorithm
Our algorithm computes the optimal future position of a CNA such that a position-information broadcast from this position by the CNA will reduce the combined position uncertainty of all AUVs by the largest amount. The algorithm is decentralized and as such only incorporates information which is locally available or overheard through the acoustic channel. Using decentralized algorithms is a key requirement in the underwater domain as the reliable communication channel to a single controller, as required by centralized topologies, is not available. As we do only use locally available information and in particular don't have any knowledge about the future SCM positions (beyond actual course and speed) we are not able to compute a globally optimal trajectory. For the remainder of this paper "optimal" thus refers to a local optimum within the set of locations which can be reached by the CNA at that time.
The metric which is minimized in this version of the algorithm is the sum of the trace differences between the prior and posterior covariances of the AUV's position estimates. This metric assumes that the navigation algorithm running on all vehicles is an EKF as described in the previous section. The algorithm however can accommodate other Bayes filters and any state representation by modifying line 6 in algorithm 2 and line 6 in algorithm 3 accordingly. Also, the metric which is minimized can be changed to other metrics by modifying line 5 in algorithm 4. The following assumptions are made by the adaptive positioning algorithm:

A. Vehicles
There are two groups of vehicles. A group of AUVs, A, which carry out a mission and a group of CNA, C, which serve as moving navigation beacons. Optimizing the relative position between CNA and an AUV is entirely left to the CNA as it is assumed that each AUV's track is solely controlled by its mission objective. No CNA needs to be aware a priori of all members of the set of participating AUVs and CNAs. The sets A and C can be updated dynamically.

1) Communication:
Each member of A and C shall be outfitted with an acoustic modem for data transmission and intra-vehicle ranging. As only one vehicle can transmit at any given time, there will be a schedule S which assigns a time slot during which a vehicle (CNA or AUV) can broadcast a status message. The schedule S is, either, provided to all vehicles before the mission starts or, in the case of a central communications controller which initializes communication through polling, the vehicles "learn" the schedule as they overhear polling requests. It is assumed that the schedule is repetitive and does not change over a longer period of time such that predictions about the time of future transmissions are possible once S is known. Each entry in S consists of a vehicle identification number, i, and a broadcast time, t b i , which is relative to the start of the schedule. When a vehicle i broadcasts, its transmission m i not only contains the actual distribution over its pose estimate x i , but also its course θ i and speed v i or even a short description of the upcoming mission plan. This information fits into a typical modem packet with the size of ≈ 40 bytes. This will enable every other vehicle overhearing this message to compute a short-time prediction of the vehicle's future position. The message also contains a unique vehicle identification number i. Each vehicle also stores the predicted positions of CNAs and AUVs in the according entries in A or C.
2) Sensors: Optionally, the CNA may have available to them a sensor table N which contains a set of tuples, in which each tuple n i ∈ N contains information about the i-th sensor's capabilities. If this information is available to the CNA it can also carry out short-term predictions about the future position and uncertainties of the AUV and CNA.
The adaptive positioning algorithm consists of four modules (Algorithm 1, 2, 3 and 4), which are run on each CNA individually when the appropriate conditions are met. Algorithm 2 and 3 both call the function algorithm 4 which computes the optimal CNA position for a given setup of CNAs and AUVs.
Algorithm 1 is run whenever the CNA receives a broadcast from an AUV.
Algorithm 2 is run whenever the CNA receives a broadcast from another CNA.
Algorithm 3 is run whenever the schedule S indicates that the CNA should broadcast.
Algorithm 4 is a function which computes an optimal future CNA position when the position and associated uncertainties of all CNAs and AUVs have been predicted for this time.

B. Message Reception from an AUV (Algorithm 1)
When a CN receives a broadcast from an AUV, a j , it decodes the message (line 3) and uses it to update its estimate of the future positions and associated uncertainties of a j up to the next time t b i (line 4) at which the CNA is scheduled to broadcast. It achieves this by forward projection using a j 's actual position course and speed (line 5) and the information about a j 's sensor quality which is retrieved from N i (j). If the received message m A j (t 0 ) from a j contains a description of its short term mission plan an even more accurate prediction can be made. For the scenario we use to illustrate the algorithm, all predictions are based on available course and speed information. The functions g(·) and h(·) in line 5 also use the information locally stored in C i so as to consider the message broadcasts from all other CNA which occur between the current time (t 0 ) and t b i and how they will affect the AUV's position estimate at the time t b i . The updated information about a j is stored in Algorithm 1 Executed on CNA whenever a message from an AUV is received.
end if 8: end loop

C. Message Reception from Another CNA (Algorithm 2)
When a message is received from CNA c j it shall contain a more recent estimate of the CNA's state estimate x C j , the associated uncertainty P C j as well as the actual course and speed (estimates) v C j and θ C j (line 3). The algorithm then locally emulates the effect that that specific broadcast would have had on the positioning estimate of all AUVs assuming that all AUVs received the message. This is carried out as follows: Firstly, it fetches the predicted position, x A k , and uncertainty estimate, P A k , for the actual time t 0 for each AUV in A i from its AUV table (line 5). It then updates the position and uncertainty of each AUV using the Kalman state update (5) and the uncertainties using the Kalman covariance update (6) (line 6) and then stores the the resultant estimate back into the table A i (k) (line 7).
Algorithm 2 then duplicates the decision making process taking place at CNA c j . Using the communications schedule S i (j), it computes the point in time, t b j , at which CNA c j will broadcast again (line 9). Calling the function compute opt CNA position (algorithm 4) with the actual position of c j obtained from m C j (t 0 ) and our Algorithm 2 Executed on a CNA whenever a message from another CNA is received.
if message m C j received from CNA c j ∈ C i then 3: for all a k ∈ A i do 5: 10: end if 14: end loop local knowledge of the future positions of the AUVs and the CNAs, we can compute the optimal position x C j opt (t b j ) for c j (line 11). If all information transmitted through the acoustic modems was received by all vehicles, then CNA c i and c j will have the same positioning information available and x C j opt (t b j ), computed locally by c j , should be the same location computed by c i . If not all values were equally shared, c i and c j will compute different values, but in the absence of any other information x C j opt (t b j ) is the best prediction for c j 's position at t b j . Additionally we use the table entry for c j 's sensor noise characteristics N i (j) to predict the future position uncertainty at x C j opt (t b j ) (line 11). The new estimate about c j 's future positions is updated in C i (j, t b j ) (line 12).

D. CNA broadcast (Algorithm 3)
When the actual time, t 0 , matches its scheduled broadcast time, t b i , CNA c i first broadcasts a message m C i (t 0 ) containing its actual position estimate x C i , associated covariance P C i as well as its actual course θ C i and speed v C i (line 3) in a similar manner to that of algorithm 2. First, the effect that this CNA's position broadcast would have on each AUV is modeled, in which it is assumed that each received the latest broadcast m C i (t 0 ) (line 5, 6 and 7). Then using the schedule S i the next broadcast time t b i is computed (line 9). At this time all available information about the positions of each CNA and AUV at t b i (from A i (t b j ) and C i (t b j )) is used to determine the optimal position, x C i opt (t b i ) at which the CNA's next Algorithm 3 Executed on a CNA whenever it is scheduled to broadcast.
for all a k ∈ A i do 5: end for 9: 10: end if 14: end loop broadcast should take place (line 10). The position uncertainty accumulated up to x C i opt (t b i ) is predicted based on the actual position and uncertainty, as well as the future position and the sensor noise N i (line 11). All updated information is stored in C i (i, t b j ) (line 12).

E. Determining the Optimal CNA Position (Algorithm 4)
This function computes the optimal CNA position for a desired time, t b i , assuming that the predicted position of all other CNAs in C i and the positions for all AUVs in A i are available.
As we showed in [14] that there is no closed form solution to find the optimal beacon point, we chose a brute-force approach. The function first computes a grid of discrete positions M which could possibly be reached by the CNA before the next broadcast (line 1). The number of grid positions in M depends on the maximum speed of the vehicle, v max , the time between now (t 0 ) and the next broadcast t b i and the spacing of the grid points. As the runtime of the function is linearly dependent on the number of grid points, the grid spacing can be varied depending on v max , t b i and the available CPU cycles. For each grid point, x C p in M , we now compute by how much the overall position uncertainty would be reduced if it would broadcast from this point at t b i . It does this by fetching the position x A k (t b i ) for each AUV a k (line 4) and computing the difference between the trace of the prior P A k (t b i ) and posterior covariance matrix P A k (t b i ), assuming a Kalman update (6) by c i from position x C p . The trace differences for all AUVs are summed up and Algorithm 4 Compute the optimal position x C opt for a CNA c i for a predicted time t b i . It assumes that the position and uncertainties for all other vehicles (CNAs and AUVs) are given by A i and C i .
for all a k ∈ A i do 4: stored in K (line 5). K has the same size as M . After the total achievable improvement has been computed for all x C p (t b i ), we determine the largest entry in K. The position which maps to this entry is the optimal position x C p opt to which the CNA should move so as to maximally reduce the uncertainty of the AUV set (line 8).

V. Results
To test this adaptive positioning algorithm we simulate two scenarios. The first scenario (  A. One AUV, one CNA Figure 2 shows the simulation results for the most basic possible CN setup, one CNA and one AUV. Every 60 seconds the CNA broadcasts its position and then computes the optimal position for the next broadcast. As there are no other CNA present, the CNA only needs to take the effect of its own updates and the vehicles' sensor performance into account. The top plot, at t=20 s, shows the situation directly after the mission commenced. The CNA has just broadcast its position and the position it predicts for the AUV at the next broadcast which is marked with red "+". The semi-transparent circle with radius r = ∆t · v max = 60 s · 2 m/s = 120 m marks all positions which the CNA could reach at maximum speed. Our algorithm discretizes this circle into grid points with 5 m spacing. It then computes, for each grid point, the position uncertainty which the AUV would have after a hypothetical update broadcast by the CNA from this grid position. The difference between the prior and posterior trace of the AUV's position estimate is represented by the color of the semi-transparent circle. Positions marked blue would lead to a very small decrease in overall uncertainty and positions marked red to a very high overall decrease. The mapping between the absolute value of K(p) and the color is scaled, each time the circle is plotted, to span the maximum color space. Thus we cannot provide a legend which maps colors to absolute values for K(p). The position which corresponds to the maximum of that difference is selected as the future position for he CNA.
As the AUV has a high variance in its heading direction it accumulates the highest uncertainty in the direction perpendicular to the direction it is traveling in. As shown by Zhou and Roumeliotis in [15], the biggest decrease in the trace of the covariance can be achieved if the beacon vehicle is somewhere along the semi-major axis of the AUV's covariance ellipse. Bruteforce computation confirms this, by highly favoring positions perpendicular to the direction in which the AUV is traveling, illustrated in dark red, for the first update. At t=72 s (middle plot) the CNA has reached its planned position. The AUV has reached its predicted position and the CNA has transmitted its message and computed a new optimal broadcast position for its new message. As the previous broadcast, at t=70 s, strongly reduced the error in the north-south direction, the along-track error will dominate the position uncertainty and the optimal position is in line with the vehicle traveling. The bottom plot, at t=320 s, shows the vehicles after the fifth broadcast. At this stage a "saw-tooth" pattern has been established, in which the CNA oscillates between the two relative positions (top and middle plot). Due to the much larger distances that the CNA has to travel in this scenario, compared to those of the AUV, the distance between the CNA and the AUV slowly increases, as reaching the optimal relative position is the CNA's only goal. Future versions of the algorithm will enforce a minimum distance between the vehicles.

B. Two AUV, Two CNA
A more complex CN-scenario is shown in figure 3. Here, two CNA try to jointly optimize their trajectory to improve the position uncertainty for two AUV. All four vehicles start at the same position and both CNA broadcast their position every 30 s. After CNA 1 broadcasts its first message, at t=10 s, it determines that the position marked by the blue "+" is the optimal position for its next broadcast. Meanwhile CNA 2 waits until its first broadcast, at t=40 s, and then determines its optimal position for its next broadcast at t=100 s (cyan "+"). When computing the trace difference represented by the semi-transparent circle in the middle plot (the corresponding circle for CNA 1 is not shown as they would overlap), CNA 2 takes the effects of the broadcast from CNA 1 at t=70 s into account, as otherwise it would also head for the optimal position previously computed by CNA 1, leading to a redundant update. Shortly after CNA 2 reaches its computed position, all four vehicles achieve the stable position of a quadrilateral which is maintained throughout the mission (bottom plot).
The "one AUV, one CNA scenario" depicted in figure 2 shows how optimizing the trajectory for the shortterm optimal broadcast position alone can lead to a sub-optimal long-term solution as the distance between the vehicles constantly grows until the distance is too long for transmission. Therefore we would also like the dynamic positioning of our CNA to be influenced by other objectives such as maintaining a minimum distance to all vehicles. If the acoustic propagation conditions are known, choosing the broadcast position such that the transmission loss to all vehicles is minimized could be another possible objective. Fusing multiple objectives is beyond the scope of this paper. However the output of our algorithm could provide an input into the IVP method proposed in [10] which would carry out the fusion.

VI. Conclusions
In this paper we propose an algorithm which allows dedicated mobile navigation beacons (CNAs) to optimally position themselves in order to best serve a group of submerged vehicles carrying out a mission (SCMs). The algorithm does not require any a priori knowledge about the vehicle's path and only uses information available from overheard broadcasts and proprioceptive sensors. It is completely distributed and can dynamically adapt to a change in the number of CNAs and SCMs. As the required update rates are low, the computational load of the algorithm is negligible. Simulations for two scenarios show that stable CNA trajectories can emerge.
Future versions of the algorithm will improve the resilience towards loss of communication by including link quality information. By optimizing the trajectory for improved intra-vehicle communication, we also enable the CNA to better serve in its secondary role as communications hub or relay.