Cooperative Control and Navigation in the Scope of the EC CADDY Project

Size: px

Start display at page:

Download "Cooperative Control and Navigation in the Scope of the EC CADDY Project"

Bennett Whitehead
5 years ago
Views:

Cooperative Control and Navigation in the Scope of the EC CADDY Project Pedro Abreu, Mohammadreza Bayat, João Botelho, Pedro Góis, António Pascoal, Jorge Ribeiro, Miguel Ribeiro, Manuel Rufino, Luís

1 Cooperative Control and Navigation in the Scope of the EC CADDY Project Pedro Abreu, Mohammadreza Bayat, João Botelho, Pedro Góis, António Pascoal, Jorge Ribeiro, Miguel Ribeiro, Manuel Rufino, Luís Sebastião, Henrique Silva Laboratory of Robotics and Systems in Engineering and Science (LARSyS), ISR/IST, University of Lisbon, Lisbon, Portugal Abstract Cognitive Autonomous Diving Buddy (CADDY) is an FP7 project initiated in February The key objectives of the project are to develop, implement, and demonstrate in the course of missions at sea the efficacy of a number of innovative systems to assist human divers during the execution of demanding scientific and commercial activities in hazardous underwater environments. At the core of CADDY is the development of an underwater robot that plays the role of a buddy diver and a surface robot to improve guidance, monitoring, assistance, and safety of the diver s mission. The present paper gives a brief summary of the work done during the first year of the project towards the development and testing of the systems required for coordinated surface / underwater vehicle operations. We consider the cases where the leader is either the surface or the underwater vehicle. The architecture adopted for multiple vehicle coordination is described. Results of tests with the MEDUSA class of autonomous marine vehicles show the efficacy of the systems developed for navigation and coordinated motion control. I. INTRODUCTION The EC-FP7 CADDY project brings to the core of its R&D programme a number of institutions with complementary expertise to advance cognitive robotics in the underwater arena; namely, to develop robots capable of cooperating with divers in challenging scientific and commercial tasks. See [1] for an overview of the present project and [2] for a description of previous related work in this area in the scope of the EC CO3AUVS project. At the core of the CADDY concept, illustrated pictorially in Figure 1, is the ability of two or more agents (vehicles and human divers) to maneuver in close cooperation by exchanging motion-related information over the acoustic channel and reacting accordingly. As an example of a cooperative maneuver we cite the case where the underwater vehicle (the robot companion) is guided by the surface vehicle along a desired trajectory. In this situation, the surface vehicle trajectory acts as a reference or leader (in 2D) and the underwater vehicle must track this reference in the horizontal plane (albeit with a desired x-y offset, if required), while undergoing independent motion in the vertical plane (e.g. staying at a constant depth or constant altitude above the seabed). This technical problem is sufficiently rich in that its solution sheds light into the solution of other cooperative motion problems that are key to the implementation of the full CADDY system. Furthermore, the problem can be simply motivated by an interesting mission scenario that can be Fig. 1. The CADDY concept referred to as the guided tour : one or more divers are interested in visiting an archaeological site, and to this effect they wish to be guided by an underwater tour guide that acts as a reference for them to follow visually, at close range. It is therefore up to the underwater vehicle to maneuver along a desired, pre-determined tour path. Because of possibly stringent navigational constraints underwater, this is done by having the underwater vehicle track the reference trajectory taken by the surface vehicle. This calls for the installation of an ultra-short baseline (USBL) unit on-board the AUV capable of measuring its relative position with respect to the surface vehicle. The resulting tracking system will henceforth

follower: based on proprioceptive motion data as well as ASV motion-rela acquired and transmitted using acoustic units (USBL and acoustic modems), it man as to track the motion of the leader.

2 follower: based on proprioceptive motion data as well as ASV motion-rela acquired and transmitted using acoustic units (USBL and acoustic modems), it man as to track the motion of the leader. be referred to as Leader Tracking System (LTS). The LTS thus defined can of course be modified to also allow for the realization of missions where the leader is the AUV and the follower (tracking the leader) is the ASV. This functionality becomes important in situations where the AUV takes over the task of guiding the diver directly (e.g. in response to foreseen events detected on line), and seeks navigation help from the surface vehicle that must track the AUV as the mission unfolds. In this case, the burden of determining the relative position between the AUV and the ASV falls on the ASV, that can either carry an USBL unit (therefore reducing the cost of the AUV) or perform sufficiently exciting maneuvers while localizing the AUV by resorting to rangebased localization strategies. In both cases, the ASV is required to transmit updates about the AUV position to the latter vehicle using an acoustic modem. It was against this background of ideas that the LTS was first chosen as a case study to illustrate the sequence of steps that go into the design, implementation, and field testing of the systems required to execute a coordinated maneuver. For this reason, the paper focuses on the design, development, and field-testing of the navigation and control systems REFERENCES required to execute Leader Tracking maneuvers. The setup adopted for the LTS is sufficiently general for applications in a large variety of 2014 scenarios. Later, in the Experimental Results section, two such scenarios will be presented. The first consists of an underwater vehicle tracking a surface vehicle; the second illustrates a surface vehicle tracking an underwater vehicle. The results on these two scenarios are a solid contribution towards achieving the necessary functionalities in terms of cooperative maneuvers envisioned in the project, since it should be possible to replace one of the vehicles with a human diver with only slight modifications to the system. II. LEADER TRACKING SYSTEM: DESIGN FRAMEWORK The Leader Tracking System (LTS) consists of a set of control and navigation-related blocks that, when combined with a strategy for exchanging information, enables one or more agents (followers) to track (follow) the position of a specific agent (leader) on a horizontal plane, with optional x y offsets defined in a convenient path-dependent manner. The path of the leader agent is unknown a-priori by the follower agents. Motion in the vertical direction is considered to be handled independently by a depth or altitude controller. The theoretical framework adopted for system design borrows from cooperative control and navigation theory, see for example [3] [10] and the references therein. See also [11], [12] for relevant related work in the scope of the EC MORPH project. From an implementation standpoint, the general scheme adopted to run a Leader Tracking maneuver can be explained by referring to the block diagrams in Figures 2 and 3 that illustrate the navigation, control, and informationflow architectures adopted. In the diagrams, the ASV is the leader and maneuvers at will (e.g., following a pre-planned path with a desired speed profile). The AUV is the follower: based on proprioceptive motion data as well as ASV motionrelated data, acquired and transmitted using acoustic units (USBL and acoustic modems), it maneuvers so as to track the motion of the leader. Later, an extension to the setup is Information Flow acoustic comms Ultra Short Baseline (USBL) system GPS Autonomous Surface Vehicle (ASV)/Leader GPS + Acoustic Modem + USBL pinger ˆṗ asv - AUV speed estimate? asv - course angle estimate? asv ˆ - course rate estimate p asv - AUV position p asv? p auv relative position error Autonomous Underwater Vehicle (AUV)/Follower GPS + Acoustic Modem + USBL (array) + Doppler Velocity Log (DVL) + Attitude and Heading Reference System (AHRS) Fig. 2. Leader tracking system (ASV leader and AUV follower): general architecture showing the motion sensor units used and the data flow over the Leader tracking system: general architecture showing the motion acoustic channel. See also Figure 3 and reference [10]. sensor units used and the flow of data over the acoustic channel introduced so as to enable the underwater agent (AUV) to play the role of leader. In what follows we give the reader an abridged description of the key blocks that go into the making of the LTS. The reader is referred to [10] for complete details. [1] P. Abreu, Sensor-based formation control of autonomous underwater vehicles, MSc Thesis, IST, Univ. Lisbo ASV Architecture. In the set-up adopted, the leader (ASV) is equipped with a GPS and runs a filter to estimate its inertial speed, course angle, and course angle rate. This information [2] A. Aguiar, A. Pascoal, Cooperative control of multiple autonomous marine vehicles: theoretical foundations issues, in Recent Advances in Unmanned Vehicles, IET, Editors Geoff Roberts and Robert Sutton, [3] J. Soares, A. Aguiar, A. Pascoal, M. Gallieri, Triangular formation control using range measurements: an is transmitted to the AUV via an acoustic modem. In its marine robotic vehicles, Proc. IFAC Workshop on Navigation, Guidance and Control of Underwa present form, this system running on the ASV is mechanized (NGCUV 2012), Porto, Portugal, April, as a Kalman Filter (KF). It uses the position measurements obtained from a GPS receiver, in an ENU (East, North, Up) reference frame, to estimate the inertial velocity of the surface vehicle in polar coordinates (norm and course angle) and the rate of the course angle. Both the norm of the velocity vector and the rate of the course angle are assumed to be constant in the model used for this KF, therefore the latter embodies in its design model piecewise constant-velocity and constantcurvature trajectories. As an alternative to this filter, when the leader vehicle (in this case the ASV) is moving along a preplanned trajectory, the nominal values of its velocity, course angle and rate of course angle can be broadcast instead. This is typically the case, as the leader vehicle usually performs path-following on a planned trajectory. This is also the case in the experimental results of the system shown in later sections. AUV Architecture. The system running on a follower vehicle (in this case the AUV) involves four main subsystems: i) ASV velocity estimator. The follower (AUV) receives the information broadcast by the leader at discrete, possibly asynchronous instants of time (with a time delay) and runs an internal filter to generate estimates of the ASVs inertial speed, course angle and rate at a rate fast enough to be used for motion control. ii) Relative position estimator. This block is in charge of fusing data available from different sources: i) the ASVs speed, course angle, and course angle rate broadcast by the ASV; ii) the (delayed) relative position of the ASV with respect to the AUV obtained using an Ultra-Short Baseline (USBL) system installed on-board, iii) optionally, the velocity of the AUV with

Leader rate ACOUSTIC COMMS leader velocity leader velocity rate Leader Velocity Estimator estimated rate estimated leader speed and course estimated relative position Formation Controller surge speed

AUV and Inner Loops Follower Leader tracking system: identification of relevant variables respect to the seabed, iv) Heading, provided by an Attitude and Heading Reference System (AHRS), and v) surge

3 Leader rate ACOUSTIC COMMS leader velocity leader velocity rate Leader Velocity Estimator estimated rate estimated leader speed and course estimated relative position Formation Controller surge speed command yaw command yaw surge speed relative position (USBL) Relative Position Estimator estimated ocean current velocity DVL (optional) Fig. 3. AUV and Inner Loops Follower Leader tracking system: identification of relevant variables respect to the seabed, iv) Heading, provided by an Attitude and Heading Reference System (AHRS), and v) surge speed with respect to the water, where the latter is estimated using a quasi-static propulsion model that relates the speed of rotation of the vehicle propellers with the vehicles forward speed. Its output consists of the estimate of the relative position of the AUV with respect to the ASV, together with the estimate of the underwater current, both at a sufficiently fast update rate. iii) Formation controller. This component is in charge of generating commands for the AUV surge speed and yaw angle (tracked by the AUV inner loops) so as to make the AUV track the position of the ASV with a properly chosen, possibly timevarying offset, as per the requirements of the maneuver being performed. In fact, the formation controller can be viewed as the cascade composition of two operations: i) generating a virtual target (determined by the trajectory described by the leader and the requisites of the leader tracking maneuver) and ii) tracking the virtual target. The generation of the virtual target (its position and velocity) relies on the fact that the follower (AUV) has access not only to the relative position and velocity of the leader (ASV), but also to the curvature of its trajectory. In the tests illustrated in later sections, this block is configured so that the virtual target is positioned at a point defined by desired along- and across-path distances with respect to the path defined by the ASV (which, we assume, is either a straight line or a segment of a circumference, or a combination thereof). This allows for the characterization of the trajectory of the virtual target, both in terms of its position relative to the underwater vehicle (follower) and its inertial velocity. These parameters are then fed to a trajectory-tracking controller that forces the actual vehicle to track the virtual one. iv) Inner loops. This block implements PID controllers that force the surge speed and heading of the vehicle to track (albeit with error) the respective commanded values. III. E XPERIMENTAL R ESULTS This sections contains the results of in-water experiments with a surface and an underwater vehicle (both of the MEDUSA-class of autonomous marine vehicles) aimed at assessing the efficacy of the systems developed for Leader Tracking in the scenarios where the leader is either the surface or the underwater vehicle. The MEDUSA vehicles can operate at the surface or underwater and are built of two acrylic tubes attached to a central aluminium frame, weighing about Fig. 4. The MEDUSA class of vehicles Fig. 5. MEDUSA vehicle showing the USBL unit in the lower-body segment 30 [Kg] each. The vehicles were developed at the Laboratory of Robotics and Systems in Engineering and Science (LARSyS)/ISR of the Instituto Superior Tecnico; see [13] for details. A. Surface Leader Experiment In what follows we summarize the results of practical experiments that illustrate the performance of the LTS in the first scenario: the ASV is the leader and the AUV is the follower. During the tests, the ASV was requested to perform a U-shaped path-following maneuver at the surface. In the set-

4 Fig. 6. Leader Tracking System with ASV as leader: trajectories of AUV and ASV up adopted, the virtual target (to be tracked by the AUV) is positioned at a point defined by desired along- and across-path distances with respect to the path defined by the AUV (which, as explained before, is either a straight line or a segment of a circumference, or a combination thereof). The along and crosspath distances were set to -17 and +5 meters, respectively (intuitively speaking, this means that the AUV was requested to stay 17 meters behind along the path and 5 meters to the right). Throughout the maneuver, the AUV was commanded to remain at a fixed depth. The nominal speed of the leader vehicle (ASV) was set to 0.3 m/s. Figure 6 shows the trajectories of both vehicles. The performance of the Leader Tracking System illustrated by these figures is visibly good. The deterioration in performance that occurs when the ASV enters or leaves the circular part of the maneuver is simply due to the fact that the along- and cross track position specification for the virtual vehicle (to be tracked by the ASV) are done considering that the circular part of the path is extended backward as a circumference (upon detection that the ASV actually entered the circular path). This was done at at time when tests were conducted with a simplified implementation of the Leader Tracking system. Meanwhile, this problem has been overcome by extending back the path taken by the AUV taking into consideration the actual path traversed, stored in memory. This is illustrated in the results shown in the next section, which no longer exhibit this problem. B. Underwater Leader Experiment The second scenario is now illustrated with results obtained with one AUV playing the role of leader, performing pathfollowing on the same pre-defined U-shaped mission, and one ASV playing the role of follower. See Figures 7 and 8. The ASV was configured to follow 5 meters behind along the path of the leader, and 5 m to the right. The nominal speed of the leader vehicle (AUV) was set to 0.3 m/s. Again, the results are visibly good, with the tracking error of the follower (ASV) generally below 1 meter. The issue identified in the previous section when the leader vehicle enters or exits the turn has been effectively addressed and the performance no longer exhibits considerable deterioration in this situation, as is patent in the previous scenario. Fig. 7. Leader Tracking System with AUV as leader: trajectories of AUV and ASV Fig. 8. Leader Tracking System with AUV as leader: Tracking error of the follower (ASV) IV. CONCLUSIONS The paper gave a summary of the work done during the first year of the EC FPT CADDY project project towards development and testing of the systems required for coordinated surface / underwater vehicle operations. This is part of a concerted effort to develop a number of innovative systems to assist human divers during the execution of demanding scientific and commercial activities in hazardous underwater environments. We addressed the case of two vehicles where the leader is either the autonomous surface vehicle (ASV) or the autonomous underwater vehicle (AUV). The architecture adopted for multiple vehicle coordination was described. Results of tests with two vehicles of the MEDUSA class showed the efficacy of the systems developed for relative navigation and coordinated motion control. Future work will address the extension of the methodology adopted to deal with the case where one or the two vehicles cooperates with a human diver.

5 ACKNOWLEDGMENT This work was supported by the EC CADDY project (FP7/ICT/2013/10) and the FCT [PEst- OE/EEI/LA0009/2011]. The authors are grateful to the partners of the CADDY project for the intensive, thought provoking exchange of ideas that have been instrumental in clarifying many concepts at the core of cooperative motion control and navigation. REFERENCES [1] N. Miskovic, M. Bibuli, A. Birk, M. Caccia, M. Egi, K. Grammar, A. Marroni, J. Neasham, A. Pascoal, A. Vasilijevic, and Z. Vukic, Overview of the FP7 project CADDY - cognitive autonomous diving buddy, in Proc. OCEANS 2015, Genova, Italy, [2] A. Birk, G. Antonelli, A. Caiti, G. Casalino, G. Indiveri, A. Pascoal, and A. Caffaz, The CO3AUVs (Cooperative Cognitive Control for Autonomous Underwater Vehicles) project: Overview and current progresses, in OCEANS, 2011 IEEE - Spain, Santander, Spain, June [3] L. Consolini, F. Morbidi, D. Prattichizzo, and M. Tosques, Leaderfollower formation control of nonholonomic mobile robots with input constraints, Automatica, vol. 44, no. 5, pp , [4] R. Cui, S. S. Ge, B. V. E. How, and Y. S. Choo, Leaderfollower formation control of underactuated autonomous underwater vehicles, Ocean Engineering, vol. 37, no. 1718, pp , [5] A. P. Aguiar and A. M. Pascoal, Cooperative control of multiple autonomous marine vehicles: theoretical foundations and practical issues, in Recent Advances in Unmanned Vehicles, G. Roberts and R. Sutton, Eds. IET, [6] J. Soares, A. P. Aguiar, A. M. Pascoal, and M. Gallieri, Triangular formation control using range measurements: an application to marine robotic vehicles, in Proc. IFAC Workshop on Navigation, Guidance and Control of Underwater Vehicles (NGCUV), Porto, Portugal, [7] J. M. Soares, A. P. Aguiar, A. M. Pascoal, and A. Martinoli, Design and implementation of a range-based formation controller for marine robots, in ROBOT2013: First Iberian Robotics Conference, ser. Advances in Intelligent Systems and Computing. Springer International Publishing, 2014, vol. 252, pp [8] F. Vanni, Coordinated motion control of multipleautonomous underwater vehicles, in MSc Thesis, IST, Univ. Lisbon, Portugal, [9] F. Rego, J. M. Soares, A. Pascoal, A. P. Aguiar, and C. Jones, Flexible triangular formation keeping of marine robotic vehicles using range measurements. in Proc. 19th IFAC World Congress, Cape Town, South Africa, [10] P. C. Abreu, Sensor-based formation control of autonomous underwater vehicles, in MSc Thesis, IST, Univ. Lisbon, Portugal, [11] J. Kalwa, A. Pascoal, P. Ridao, A. Birk, M. Eichhorn, L. Brignone, M. Caccia, J. Alvez, and R. Santos, The European R&D-project MORPH: Marine robotic systems of self-organizing, logically linked physical nodes, in IFAC Workshop on Navigation, Guidance and Control of Underwater Vehicles (NGCUV), Porto, Portugal, [12] P. C. Abreu and A. M. Pascoal, Formation control in the scope of the MORPH project. Part I: Theoretical foundations, in IFAC Workshop on Navigation, Guidance, and Control of Underwater Vehicles (NGCUV), Girona, Spain, [13] J. Ribeiro, Motion control of single and multiple autonomous marine vehicles, in MSc Thesis, IST, Univ. Lisbon, Portugal, 2011.

Autonomous Marine Robots Assisting Divers

Autonomous Marine Robots Assisting Divers LUST Overview UNIVERSITY OF ZAGREB Faculty of Electrical Engineering and Computing Department of Control & Computer Engineering Laboratory for Underwater Systems and technologies Autonomous Marine Robots