Mobile beacon control algorithm that ensures observability in single range navigation

Transcription

1 Preprints, 1th IFAC Conference on Control Applications in Marine Systems September 13-16, 216. Trondheim, Norway Mobile beacon control algorithm that ensures observability in single range navigation Filip Mandić Nikola Mišković Narcís Palomeras Marc Carreras Guillem Vallicrosa University of Zagreb, Faculty of Electrical Engineering and Computing, LABUST - Laboratory for Underwater Systems and Technologies, Unska 3, Zagreb, Croatia ( {filip.mandic; nikola.miskovic}@fer.hr). Computer Vision and Robotics Institute, Universitat de Girona Parc Cientfic i Tecnolgic UdG, 173, Girona, Spain. ( marc.carreras@udg.edu; npalomer@eia.udg.edu; gvallicrosa@eia.udg.edu). Abstract: Mobile beacon vehicles are used as navigational aid for autonomous underwater vehicles when performing navigation using single range measurements. They remove the constraints imposed on the underwater vehicle trajectory by executing trajectory that provides persistent range measurements. In this paper, control algorithm for beacon vehicle which ensures observability of the underwater vehicle s navigation filter is presented. It is characterized by small communication overhead, low computational complexity and it is deployable on both fully actuated and underactuated vehicles. The algorithm was tested in real life environment and the acquired experimental results show that despite the presence of uncertainties and communication delays, the algorithm accomplishes its task. Keywords: Navigation, Autonomous vehicles, Kalman filters, Observability indices, Marine systems 1. INTRODUCTION Underwater localization and tracking of underwater targets presents a great challenge in marine robotics due to absence of global positioning signals that are usually available in areas reachable by satellites. In order to tackle this problem, acoustic based sensors such as LBL (long baseline), SBL (short baseline), USBL (ultra short baseline) are used for underwater localization and navigation, by triangulating responses obtained from acoustic beacons. While LBLs require inconvenient deploying of underwater beacons around the operational area, USBLs that enable relative underwater localization using acoustic propagation are most often used for tracking underwater objects. Both systems are either difficult to deploy or expensive. In order to overcome these problems in some situations, a navigation method of using range measurements from a single beacon can be applied. The general assumption is that an AUV is trying to navigate underwater (determine This work is supported by the European Commission under the FP7 ICT project CADDY Cognitive Autonomous Diving Buddy (Grant Agreement No ) and ARCHROV project (DPI C3-3-R). Joint results were obtained as a result of staff exchange within H22 TWINNING project EXCELLABUST - Excelling LABUST in Marine Robotics (Grant Agreement No ). Filip Mandić is financed by the Croatian Science Foundation through the Project for the young researcher career development. its position in the global frame) by using proprioceptive sensors (DVL and/or inertial measurements) and range measurements from the beacon that is stationary or mobile at the surface and knows its absolute position. Nonlinear systems can be poorly or non-observable along specific state and output trajectories. Ensuring the observability of range-only navigation systems is an important issue and there is a great number of papers dealing with that specific topic (Batista et al. (211); Crasta et al. (213); Arrichiello et al. (215)). In order to estimate its position, the vehicle that is navigating using range measurements from a stationary beacon needs to preform trajectories that provide enough information. In Arrichiello et al. (213), observability metric for range only navigation based on the condition number of the observability matrix is used to characterize such trajectories. The action of executing an informative trajectory clearly distracts the AUV from performing its original mission. In Böhm et al. (28), approaches to avoid weakly observable trajectories in the frame of nonlinear predictive control are presented. By using term in the cost functional that penalizes weakly observable trajectories and thus leads to avoidance of weakly or non-observable regions of operation, trade off between following predefined trajectory and good degree of observability is achieved. In order to completely avoid that trade off, it is left to the beacon to ensure informative measurements, thus resulting in the scenario of mobile beacons, which is considered in this paper. There is a number Copyright 216 IFAC 48

2 of ways of determining the optimal trajectory of a mobile beacon. In Moreno-Salinas et al. (213), the optimal trajectory to ensure observability of static target is found by maximizing Fisher Information Matrix determinant, while in Tan et al. (214) dynamic programming and Markov decision process approach is used. The goal of the algorithm presented in this paper is to steer the mobile beacon in order to decrease the localization error of the single range navigation system by using an online algorithm with the very low computational and communication demands. The proposed method is particularly interesting for underwater applications because of the limited bandwidth of acoustic communications. In the control loop, the only data that needs to be transmitted over the acoustic link is the current cost function value which is sent from the vehicle to the beacon, and the current beacon position data. Optionally, beacon velocity can be sent in order to improve localization performance. Furthermore, the proposed algorithm does not require knowledge of the target vehicle s trajectory in advance. That can be particularly useful in real life conditions when planned trajectories can be altered due to some unexpected situations. The main motivation for the presented work arises from the FP7 CADDY - Cognitive Autonomous Diving Buddy project that has the main objective to develop a multicomponent marine robotic system comprising of an autonomous underwater vehicle (AUV) and an autonomous surface marine platform that will enable cooperation between robots and human divers. In the context of the CADDY project one of the main prerequisites for executing envisioned control algorithms and ensuring diver safety during human-robot interaction is diver position estimation. Single range measurements can be used in order to achieve this requirement. The movement of the diver is usually slow and unpredictable, therefore all the techniques which use a priori knowledge of the target trajectory are not the best choice for determining a beacon s trajectory that makes system observable. The proposed algorithm is especially suited for such application. This paper is organized as follows. Section 2 describes the concept of using a mobile beacon, guided by the proposed control scheme, to increase the underwater vehicle s quality of navigation by increasing degree of observability. Also the observability cost function is presented. In Section 3 control scheme is described. Experimental results acquired during field trials are given in Section CONCEPT DESCRIPTION The vehicle has to perform a sufficiently informative trajectory in order to estimate its position using single range measurements. This can prevent the vehicle from doing other useful activities which require trajectories that are not informative enough. To avoid that, an approach with two vehicles, where one of them is a beacon, can be used. In that case, a mobile beacon, which knows its position accurately (from GPS), is responsible for assuming a trajectory which will provide informative range measurements for the target vehicle s navigation filter. Beacon (surface vehicle) Beacon dynamics Vehicle dynamics ẋ bref ẏ bref x b, y b Control scheme Kalman J(P ) Target (underwater vehicle) Fig. 1. The concept of observable trajectory tracking using extremum seeking based control scheme. Dashed signals represent acoustic communication. Figure 1 depicts the main idea which enables better vehicle position estimation by using single beacon measurements. The mobile beacon sends its position (x b, y b ) to the vehicle s Kalman filter used for navigation. Optionally, the beacon s velocity (ẋ b, ẏ b ) can be sent in order to improve localization performance. Information generated in the target vehicle s navigation filter is then used to calculate cost function value J which gives a measure of observability. The current cost value is then sent to mobile beacon which tries to minimize it online by using an extremum seeking scheme which steers the mobile beacon towards the minimimum of the cost function. The beacon again sends its position to the vehicle, thus closing the control loop. The range measurement used for determining the vehicle s position is acquired during the acoustic communication cycle. We consider the state space representation of vehicle beacon system relative position in horizontal plane, given with (1). [ ẋ ẏ ] = [ ] vx + ζ (1) v y The state vector is given with x = [ x y ] T where x = x x b and y = y y b are relative positions. The inputs v x and v y are relative speeds between vehicle and beacon in Earth fixed coordinate frame produced by actuators, sea currents or some other disturbances that act on the vehicle and the beacon, while ζ R 2 1 represent process noise. Depending on the vehicle s sensor configuration, some of the quantities acting on the vehicle cannot be measured, so they can be considered as process noise. In order to execute the proposed algorithm, the beacon vehicle does not need to know the target vehicle s dynamic and kinematic properties and there are no special requirements on the vehicle or beacon dynamics necessary to execute the algorithm except that the beacon s absolute maximum velocity must be much higher than the target velocity. The system measurement is represented by the Euclidian norm between the vehicle and the beacon position as: r = x 2 + y 2 + ν, (2) where ν N (, σ) is measurement noise modelled as Gaussian white noise with variance σ. Equation (2) represents the range in horizontal plane, however, the acoustic J 49

3 range measurements are acquired by acoustic modems in a three dimensional space. Since depth sensors are affordable, range measurements in horizontal plane are easily calculated as r = R 2 z 2 where R is measured range and z is target vehicle depth. Achieving observability of system (1) ensures that relative distance between the vehicle and the mobile beacon in the respective directions can be successfully estimated. The beacon position is known and sent to the target vehicle via acoustic link, therefore determining target vehicle position is straightforward. 2.1 Observability cost function In order to steer the mobile beacon along a trajectory for which the system (1) is observable, the observability needs to be measured. The rank condition, usually used in determining observability of a system, does not give information on the quality of observability, only whether the system is observable or not. In Krener and Ide (29), the local unobservability index and the local estimation condition number were introduced to measure the degree of observability of a system. The local unobservability index is reciprocal of the smallest local eigenvalue and it gives information on how difficult it is to estimate the initial condition from the output. Local estimation condition number is defined as κ (W) := λmax(w) λ min(w), where W is the Observability gramian and λ max and λ min represent the maximum and minimum eigenvalues, respectively. The estimation problem is ill conditioned when the condition number is much larger than 1 because that indicates that some outputs are more sensitive to small changes of the initial condition in one direction. A cost function based on rank condition that enhances observability of system (1), given with (3), is derived in Mandić et al. (215), where the link between the system s observability Gramian W and estimation covariance matrix P for the system given with (1) and (2) is established. It is shown that the problem of maximizing the minimal eigenvalue of the observability Gramian W corresponds to minimizing the maximal eigenvalue of the estimation covariance P. J (P) = [T r (P)] 2 4 detp. (3) The minumum of the cost function, as it is defined, is achieved when the eigenvalues of the matrix P are equal, meaning that localization error in respective directions is the same. The single range measurement can be taken from only one direction at the time, therefore that minimum value cannot be obtained and the goal of the proposed control algorithm is to only reduce the cost as much as possible. The algorithm ensures that the system is always observable, hence the matrix P should never become singular. The cost function (3) is very convenient for use since a navigation filter is necessary for vehicle s localization when using single range measurements. Due to that, the covariance matrix P is already available aboard the vehicle and the proposed cost criterion added computational complexity is low. 3. CONTROL SCHEME In order to make the mobile beacon achieve a trajectory with good observability properties, while navigating using ẋ bref ẏ bref ẋ b = f b (x b, u b, t) k k cos ωt sin ωt x b, y b γ x t, y t J(P) Pre filter Fig. 2. Control scheme employed for steering mobile beacon in 2D plane. ψ bref u bref ẋ b = f b (x b, u b, t) ωt mod 2π k x b, y b γ x t, y t J(P) Pre filter Fig. 3. Control scheme employed for steering underactuated mobile beacon in 2D plane. single range measurements, an extremum seeking inspired scheme, shown in Fig. 2, is proposed. Looking at the scheme, and ignoring pre filter block, first the cost signal J is demodulated by multiplication with two orthogonal sinusoidal perturbations with angular frequency ω. Demodulated signals are then multiplied by a constant gain k in order to get velocity reference for the beacon vehicle in respective directions. Consequently, the beacon movement influences change of the cost function, thus closing the control loop. Unlike extremum seeking control, as presented in Krstic and Wang (2), in the presented implementation there is no control input perturbation signal and no gradient estimation of the static map. The control scheme exploits a special behavior exhibited by the cost function. Presence of the process noise matrix Q is an integral part of the Kalman filter design. It accounts for possible state model errors and in the presented case it enables variation of state variables which are assumed to be constant. The process noise increases covariance with time, which directly influences the proposed cost function in a way that, when the vehicle trajectories have bad degree of observability, the cost function grows unbounded and therefore introduces persistent excitation in system. With that in mind, it is possible to explain the algorithm operation. Fig. 3 represents an equivalent scheme to the one in Fig. 2 where the control reference inputs are the beacon s heading and surge speed. Such implementation is suitable for underactuated vehicles. From this scheme it is much more intuitive to see how the algorithm achieves its goal. We assume that the target is static. In order 5

4 to achieve maximum conditioning of the observability Gramian, the relative position and velocity vectors need to be orthogonal (Arrichiello et al., 213). If we define angle of bearing vector between beacon and the target as θ = atan2( y, x) and the angle of relative velocity vector as β = atan2( ẏ, ẋ), then angle between these two vectors is defined as α = β θ. The degree of the observability of the system defined with (1) and (2) is the highest when α = π 2 rad. A circular beacon trajectory around the target will be the one resulting in highest degree of observability. In the control scheme, circular motion is achieved by using sinusoidal demodulation of cost signal. However, it is still necessary to explain how center of the circular motion is converging towards the target. Initially, beacon starts executing circular motion with surge speed defined by current cost value and constant yaw rate, which causes angle α to change. When α is near π 2 i.e when the bearing vector between the beacon and the target and the relative velocity vector is nearly orthogonal system is observable and the cost function decreases. During one perturbation period the bearing and relative velocity vectors are orthogonal at the at least two points. These two points are the minimum and maximum range distance during the current demodulation signal period. When the beacon is in the minimum range point cost decreases the most and consequently beacon speed decreases significantly at that point while still maintaining constant yaw rate. The beacon then points away from the target at lower speed along a trajectory with the lower degree of observability which causes the cost to rise again. Due to that, on average, the beacon vehicle approaches the target faster than it departs the target i.e. the beacon remains close to the underwater target. 3.1 Control input saturation In real life implementation vehicle constraints are an important consideration. In the control scheme, the control input reference magnitude is proportional to the cost value. In order to avoid saturation of control input, in the cases when the cost is high, additional filtering is used, thus the pre filter block in the Fig. 1. A high pass filter implementation, given with (4) eliminates low frequency cost changes. In that way the signal γ value is always bounded and by selecting appropriate gain value k control reference input does not go into saturation. However, an additional constant offset c is needed in order to shift γ value into positive domain. This value is proportional with surge speed which should be always positive. Additionally, value of offset c determines the beacon s surge speed when stationary state is achieved. γ = 4.1 Experimental setup ( 1 ω s + ω ) J (P) + c (4) 4. EXPERIMENTAL RESULTS The open sea experiments, during which the presented algorithm was tested, were conducted in March 216 in Spain during EXCELLABUST project staff exchange. The experimental setup consisted of two autonomous underwater vehicles, shown in Fig. 4: Girona 5 and (a) Beacon vehicle, Girona 5. Fig. 4. Vehicles used in the experiment (b) Target vehicle, Sparus II Target trajectory Beacon trajectory Fig. 5. Target and beacon trajectory. Sparus II. Both vehicles were developed at the Underwater Robotics Laboratory of the University of Girona, Spain (Ribas et al., 212; Carreras et al., 215). Girona 5 vehicle acted as the beacon vehicle, while Sparus II was the target vehicle. The Girona 5 configuration used in the experiment is the five thruster setup: two thrusters actuate surge and yaw, and two actuate the heave degree of freedom. An additional, lateral thruster, is used in the presence of currents, or when the task at hand demands the capacity of executing lateral movements. However, in order to execute beacon algorithms only two thrusters, those actuating surge and yaw, are necessary. Acoustic modems used for communication and ranging were installed aboard the both vehicles. The beacon s position and velocity vectors, and the the cost were exchanged over acoustic link. Also, USBL measurements from a stationary mounted USBL were used as ground truth for underwater vehicle position. All the computations were done online, aboard the vehicles. 4.2 Results During the experiment the underwater target vehicle executed the trajectory shown in Fig. 5 at the constant depth of 3 meters. During the experiment the gain parameter was set to k = 4, the offset value to c =.1 and a perturbation rad s frequency of ω = 2π 8 was selected. The parameters were manually tuned to achieve satisfying performance. In real life experiments, the perturbation frequency selection 51

5 Cost Cost J t[s] 6 7 Cost filtered γ 8 9 Range [m] Fig. 6. Observability cost value and filtered cost value during experiment. Range t[s] Fig. 7. Range measured on target vehicle. 55 Target Target estimate Beacon Beacon estimate Target Target estimate 5 6 Beacon 7 Beacon estimate (a) Estimate. 1.5 Beacon error (a) Estimate. Target error 8 4 Target error Beacon error (b) Estimate error. (b) Estimate error. Fig. 8. Target and beacon vehicle north coordinate estimate and error. Fig. 9. Target and beacon vehicle east coordinate estimate and error. [rad] depends on acoustic communication constraints. With a higher perturbation frequency faster reaction to target position changes can be achieved, however in that case it is necessary to have a high acoustic update rate in order to capture cost changes. During the experiment the mean communication period was 3.4 s. α t [s] Figure 5 shows the executed target and beacon trajectories. Purple arrows over the beacon trajectory represent the beacon velocity vector at respective positions. It is immediately visible that the beacon vehicle is executing a circular motion which is known to be observable while tracking the underwater target movement. The observability cost J and the filtered cost γ calculated on the target vehicle are shown in Fig. 6. Initially the cost value is high and by executing the algorithm the cost decreases and stays bounded during the whole experiment, regard- Fig. 1. Angle between bearing and relative velocity vectors. less of the target movement. The filtered cost does not contain low frequency changes and it is bounded at all times regardless of the cost value. This feature solves the problem of control input saturation. The range between the vehicles is shown in 7. Towards the end it is possible to observe that the range is stabilizing meaning that the 52

6 beacon vehicles, which, coupled with the fact that it is not necessary to know the target vehicle trajectory in advance and the low computational demands, makes it a good choice in enhancing single range navigation of slow moving underwater targets. 18 ACKNOWLEDGEMENTS The authors would like to thank CIRS laboratory members for their help in conducting the experiment Fig. 11. Angular distribution of acquired range measurements. circular trajectory around the target is being established. Such trajectory yields highest degree of observablity which is confirmed by achieved cost value. In figures 8 and 9, the underwater target estimate calculated from available range measurements and the beacon data acquired through the acoustic channel is shown. The position is estimated by an extremely simple relative distance navigation filter whose covariance matrix P is used for calculating the observability cost. The beacon s position estimates, estimated on the target vehicle in order to determine target s absolute position, are also shown in Fig. 8 and 9. The beacon position is estimated using the assumed beacon speed in the interval between two adjacent measurements. When measurements are not available for a long time period, spikes in the estimate are present and the longer the position data through acoustic channel is unavailable, the estimate error is larger. Looking at individual position coordinates it is visible that the beacon tracks the target vehicle position changes while circulating. Figure 1 shows how the angle between bearing vector and the relative velocity vector α is converging towards value α = π 2 rad while Fig. 11 shows at which angle α are individual range measurements acquired during the experiment. Both results show that most of the measurements are taken when the bearing and velocity vector are close to orthogonal therefore confirming that the algorithm is accomplishing its task. Some range measurements are taken at a relative angle slightly larger than π 2 rad due to delayed cost but also because target is always moving. In reality, the assumption of Gaussian white noise of acoustic range measurements is not realistic and noise depends on, among the other things, the distance that acoustic waves need to travel, Moreno-Salinas et al. (213). Although the main goal of the algorithm is to enhance underwater target localization, as a consequence it also decreases range between target and beacon in horizontal plane i.e the beacon is tracking target. Due to the reduced distance, measurement noise levels should be diminished CONCLUSION The experiment has shown that, even in the presence of unknown vehicle dynamics, unknown disturbances and delayed acoustic measurements, the proposed algorithm steers the beacon vehicle towards a circular trajectory around the target vehicle, reduces cost value and improves target localization. The presented control scheme can be implemented on both fully actuated and underactuated REFERENCES Arrichiello, F., Palma, D.D., Indiveri, G., and Parlangeli, G. (215). Observability analysis for single range localization. In OCEANS Genova, 1 1. Arrichiello, F., Antonelli, G., Aguiar, A.P., and Pascoal, A. (213). An observability metric for underwater vehicle localization using range measurements. Sensors, 13(12), Batista, P., Silvestre, C., and Oliveira, P. (211). Single range aided navigation and source localization: Observability and filter design. Systems & Control Letters, 6(8), Böhm, C., Findeisen, R., and Allgwer, F. (28). Avoidance of poorly observable trajectories: A predictive control perspective. {IFAC} Proceedings Volumes, 41(2), th {IFAC} World Congress. Carreras, M., Candela, C., Ribas, D., Palomeras, N., Mag, L., Mallios, A., Vidal, E., Vidal,., and Ridao, P. (215). Testing Sparus II AUV, an open platform for industrial, scientific and academic applications. Instrumentation Viewpoint, (18), Crasta, N., Bayat, M., Aguiar, A.P., and Pascoal, A.M. (213). Observability analysis of 2d single beacon navigation in the presence of constant currents for two classes of maneuvers. In Control Applications in Marine Systems, volume 9, Krener, A.J. and Ide, K. (29). Measures of unobservability. In Decision and Control, 29 held jointly with the 29 28th Chinese Control Conference. CDC/CCC 29. Proceedings of the 48th IEEE Conference on, Krstic, M. and Wang, H.H. (2). Stability of extremum seeking feedback for general nonlinear dynamic systems. Automatica, 36(4), Mandić, F., Mišković, N., and Vukić, Z. (215). Range only navigation maximizing system observability by using extremum seeking. IFAC-PapersOnLine, 48(16), th IFAC Conference on Manoeuvring and Control of Marine Craft MCMC 215, Copenhagen, August 215. Moreno-Salinas, D., Pascoal, A., and Aranda, J. (213). Underwater target positioning with a single acoustic sensor. 9th IFAC Conference on Control Applications in Marine Systems, Osaka, Japan. Ribas, D., Palomeras, N., Ridao, P., Carreras, M., and Mallios, A. (212). Girona 5 AUV: From Survey to Intervention. IEEE/ASME Transactions on Mechatronics, 17(1), Tan, Y.T., Gao, R., and Chitre, M. (214). Cooperative path planning for range-only localization using a single moving beacon. Oceanic Engineering, IEEE Journal of, 39(2),