Autonomous Underwater Vehicle Guidance, Navigation, and Control Autonomous Underwater Vehicle Guidance, Navigation, and Control

A considerable volume of research has recently blossomed in the literature on autono- mous underwater vehicles accepting recent developments in mathematical modeling and system identification; pitch control; information filtering and active sensing, including inductive sensors of ELF emissions and also optical sensor arrays for position, velocity, and orientation detection; grid navigation algorithms; and dynamic obstacle avoidance among others. In light of these modern developments, this article develops and compares integrative guidance, navigation, and control methodologies for the Naval Postgraduate School ’ s Phoenix, a submerged autonomous vehicle. The measure of merit reveals how well each of several methodologies cope with known and unknown disturbance currents that can be constant or harmonic while maintaining safe passage distance from underwater obstacles, in this case submerged mines.


Introduction
The Naval Postgraduate School's consortium for robotics and unmanned systems education and research (CRUSER) uses three autonomous underwater vehicles, the Remus, Aries [1], and Phoenix [2] vehicles to enhance education and research. The oldest vehicle, Phoenix [3] is used in this study to investigate integrated methodologies [4] for vehicle guidance, navigation, and control through a field of obstacles amidst unknown ocean currents that can be approximated by steady state, fixed disturbance ocean velocities, and can also be represented by harmonically oscillating velocities. This integrated approach is a natural extension of the recent innovations. The Phoenix vehicle's nominal mathematical modeling was articulated in the 1988 article [5] using surge motion to perform system identification. Recent innovations [6][7][8][9][10] have extended and improved the nominal system identification resulting in highconfidence mathematically modeling in computer simulations. Such simulations permitted Wu et al. [11] to redesign the L1 adaptive control architecture for pitch-control with antiwindup compensation based on solutions to the Riccati equation to guarantee robust and fast adaption of the underwater vehicle with input saturation and coupling disturbances and the approach was applied to the pitch channel alone. Stability was emphasized in the singlechannel approach to emphasize dynamic nonlinearities and measurement errors. The Riccati equation is also utilized in this research and proves effective when applied to all six degrees of freedom per [4], where the approach is applied to instances of disturbances that are constant with simultaneous harmonic disturbances simulating unknown ocean currents and waves. In addition to these recent achievements in control, improvements have also been made to guidance and navigation. In recent years, Bo He et al. [12] demonstrated in simulations and open water experiments, the ability to overcome weak data links and sparse navigation data using a technique called extended information filter (EIF) applied to simultaneous localization and mapping (i.e. "SLAM") that proved computationally easier to implement than traditional extended Kalman filter (EKF) SLAM. Low computational cost is emphasized here to keep the vehicle size low, but also to exaggerate the laudable goal of achieving optimal or near optimal results with methods that are simple. Such is an overt goal of the new research presented here.
Just last year, Yan et al. [13] integrated the navigation system using a modified fuzzy adaptive Kalman filter (MFAKF) to combine traditional strap-down inertial navigation with OCTANS and Doppler velocity log (DVL) to navigate the challenging polar regions where rapidly converging earth meridians and challenging ocean environments filled with submersed obstacles. This benchmark achievement requires the research here to utilize similar challenging ocean conditions, and provide the motivation for selection of simultaneous steady-state ocean currents together with sinusoidal varying unknown wave conditions amidst an ocean filled with obstacles (where here the non-polar ocean is used, so mines are added to fulfill the role of malignant submersed obstacles). Furthermore, simplified waypoint guidance is derived, based on the onboard-calculated distance from the vehicle to a submerged obstacle. The simplified waypoint guidance is proven effective, and should be considered in situations where onboard operation of a modified fuzzy adaptive Kalman filter proves to be computationally prohibitive. The distance to an underwater obstacle was measured by Wang et al. [14] with a novel method: measuring extremely low frequency (ELF) emissions with onboard inductive sensors. Such emissions are produced by ship hulls with relatively pronounced amplitudes compared to small subsurface obstacles, but the harmonic line spectra and fundamental signal frequency relate directly to the closing speed of approach to the obstacle. Experiments proved that even such small signals were detectable at long range with high sensitivity and low-noise sensors of the current state of the art, thus closing distance to obstacles may now be presumed to be known passively, permitting the simplified waypoint guidance proposed in this manuscript. Particularly after ELF queuing, position, orientation, and velocity of obstacles may be monitored optically as developed by Eren et al. [15], and these states may be used as feedback signals together with the waypoint guidance (desired trajectory) permitting augmentation with linear quadratic Gaussian techniques, as done in this manuscript where full-order state observers are together optimized with attitude controller gains, followed by demonstration that reduced-order observers may also be optimized allowing vehicle operators to compensate for individually failed or degraded sensors, or instances where optimally estimated signals are superior to sensor signals in an individual or multiple channel.
Integrating these latest technological developments was demonstrated last year by Wei et al. [16], who integrated the Doppler velocity methods for obstacle monitoring into a dynamic obstacle avoidance scheme for collision avoidance. Following data fusion, a collision risk assessment model is used to avoid collisions, and claims to be effective in unknown dynamic environments, although the experiments did not go so far as to stipulate near-constant ocean currents in addition to harmonic wave actions. These challenging dynamic environments are addressed in this manuscript as a natural extension of the current state of the art.
Autonomous vehicle angular momentum control of rotational mechanics may be achieved using control moment gyroscopes, one potential momentum exchange actuator with a long, historic legacy actuating space vehicles, where mathematical singularities have just recently been overcome [17][18][19][20][21][22][23], permitting use of the actuator for underwater vehicles as done recently achieved by Thorton et al. [24,25] including combined attitude and energy storage control. These developments suffice to reveal that attitude control is not controversial, and thus the remainder of this manuscript focuses on guidance and navigation with a residual necessity to implement nominal, effective pitch and yaw control.

Materials and methods
Submersible vehicles require control systems to guide the vehicle around obstacles that can present dangers to vehicle health and safety in the presence of ocean currents. The challenge addressed here is to navigate the Naval Postgraduate School's Phoenix submersible vehicle ( Figure 1) through a minefield whose dimensions are 200 m Â 5100 m in the presence of 0.5 m/s ocean currents. The field will contain at least 30 mines placed at locations using a random number generator. The resulting controller structure has an inner-outer loop structure, and several technologies will be described including pole-placement designs, linear-optimal (quadratic) Gaussian techniques, full and partial order observers for online disturbance identification for ocean currents (both constant lateral underwater ocean currents and also sinusoidal varying currents), tracking systems and feedforward control designed to counter open ocean currents, in addition to integral control. The outer loop controller uses Line-of-Site (LOS) guidance to provide a heading command to the inner loop. The inner loop controller uses output heading feedback to track heading commands. The vehicle is simulated to traverse the minefield and successfully travels no closer than 5 m from any mine and arrive within one half meter from the commanded destination autonomously.

System dynamics
The equations of motion used to simulate the dynamic behavior of the autonomous submersible vehicle in a horizontal plane are listed in Eqs. (1)-(4). All variables in these equations are assumed to be in nondimensional form with respect to the vehicle length (7.3 0 ) and constant forward speed ($3 ft./s). The vehicle weighs 435 lbs. and is neutrally buoyant. Time is nondimensionalized such that 1 s represents the time it takes to travel one vehicle length ( Figure 2).
In addition to the following dependent equation The constant definitions in the mass m, mass moment of inertia with respect to a vertical axis that passes through the vehicle's geometric center (amidships) I z , position of the vehicle's center of gravity (measured positive forward of amidships) x G , with the remaining terms referred to as the hydrodynamic coefficients. These constants are all presented in nondimensional form.
Defining the state vector x f g ν r ψ y f g T and the control u f g δ s δ b f g T and assuming small angles, the dynamics expressed in Eqs. (1)-(4) may be expressed in state space form The system may also be expressed in a transfer function ratio of outputs divided by inputs in Laplace form using Eq. (7) where observer matrix [C] is merely a proper identity matrix to this point of the manuscript. Eq. (7) yields two transfer function relationships between each of the two possible rudder inputs as seen in Eqs. (8) and (9). Notice that both transfer functions have poles and zeros at the origin, while pole-zero cancelation is possible in the case of the stern rudder. On the other hand, even after pole-zero cancelation in the bow rudder Eq. (9), there remains an open loop pole at the origin that must be dealt with during control design, since it represents a potentially unstable element (at the very least, in the instance where the estimated constants are exactly correct, and these equations of motion exactly describe the system, an oscillatory element exists that will not decay). Nonetheless, the dynamics accord to nature. Consider trying to steer a row-boat using the rear rudder. It is much more stable than trying to steer the rowboat using a rudder in the front. This analogy applies to the submersible vehicle and is verified in these results.
In Figure 3, the uncontrolled system is analyzed by merely performing a circular turn with each (and then both) rudders. The bow and stern rudders alone are each compared to the combined use of both bow and stern rudders. The bow rudder was deflected +15 for about 21 s, while the stern rudder was deflected for À15 for about 11 s. When both rudders were deflected the maneuver was completed in roughly 8 seconds. Two initial conditions for the sway velocity were . In all cases, the bow rudder alone performed the poorest, with the stern rudder alone performing the turn in a smaller radius and shorter time. Furthermore, the combined use of both rudders resulting in tightest maneuver.
Two simulation methodologies were used to investigate sensitivities to integration method. MATLAB was used with Euler integration, while SIMULINK was used with Runge-Kutta integration with identical timesteps, Δt = 0.1 s. The results were nearly negligible and are displayed in Table 1, from which insensitivity to integration approach is established.

Control law design
In the system analysis, the optimal rudder implementation scheme was determined to be the application of both rudders, where the rudders were slaved to the same maneuver angle magnitude with the opposite sign, i.e. a "scissored-pair" per Eq. (10). In the case where only variable y is to be measured, the new state space formulation of the system equation components are in Eq. (11). Under the assumption of rudders constrained to behave as a scissoredpair the transfer function from rudder input to output y is given by Eq. (12) whose poles and zeros are listed in Eq. (13), with Eq. (14) revealing the system's eigenvalues, noting the values are identical to the location of the poles in accordance with theory. The controllability and observability matrices ([CO] and [OB] respectively) are listed in Eq. (15) (whose matrix product [OC] is in Eq. (16)) verifying these system equations are both controllable and observable, since these matrices are full rank, while the determinant of the controllability matrix is 63.1778, a large value with a small value of the matrix condition number, 13.4513. The nonzero determinant of the controllability matrix proves controllability, but to see how close the system is to being uncontrollable, the matrix condition number proves more useful. These two figures of merit indicate the system equations are highly controllable, and accordingly this manuscript will investigate and compare several options for navigation control: pole placement, linear quadratic optimal control, linear quadratic Gaussian, and time optimal control. The same holds true for observability, and thus linear quadratic Gaussian. The matrix product [OC] is the same for every definition of state variables for the given system. ,  (17) may be used to verify a diagonal matrix of eigenvalues [Λ], and then write the system of equations in normal-coordinate form x0g For the pole placement proportional-derivative (PD) controller articulated in Eq. (19), the poles are set to have roughly the same time constant, while avoiding exactly coincident poles. Gains are iterated for various time constants as displayed in Figure 4, but the following rule of thumb is asserted as well to quickly achieve performance that closely mimics the performance of linear-quadratic optimal (LQR) gains where the control effort and tracking error are equally weighted in the cost function of the optimization.
RULE OF THUMB: Select unity time-constant t c to roughly locate closed-loop poles per Eq. (20). Then place other poles at slightly different locations (e.g. s p ¼ s 1 AE 0:01∀p) The Next, the initial feedback control design was evaluated in simulations where the ship is initially located off the desired track by one ship's length port side with zero heading, and rudder deflection was limited to 0.4 radians ($23 ). Next, another simulation was performed to test an initial heading angle of 30 starboard where the initial y(0) = 0. The results are displayed in Figure 5(a) and (b) respectively. All state variations were plotted in Figure 4, highlighting the fact that y converges to zero along with the other states. Furthermore, the results of rudderlimited simulations are displayed in Figure 6 and Figure 7 for both scenarios ( Table 2).

Observer design
To design a state observer, the system must be observable [4],   Reminder: state definition x f g ν r ψ y f g T .
Assuming that only ν measurements are available, a mathematical model of the estimated system is in Eq. ½ gains of the observer may be chosen as desired for systems that prove observable, such that the error vector will converge to zero for any stable A ½ À K e ½ C ½ . In the following paragraphs, L ½ is designed by solving the matrix Ricatti equation leading to linear quadratic optimal gains, and also by solving the rule of thumb relationship between gains and time constant as done for the controller gains ( Table 3).

Reduced-order observer design
Assuming that some measurements are available from sensors, this paragraph describes the possible iterations and reveals states that are relatively more important to measure with sensors. Four possible output matrices are used to investigate observability. Four options for output matrices C ½ i for i = 1…4 result in four reduced-order observers OB ½ i for i = 1…4 are detailed in Eqs. (26)- (29). Output matrix C ½ 1 produces an observability matrix OB ½ 1 with rank = 4 (observable) and determinant not nearly equal to zero. Output matrix C ½ 2 produces an observability matrix OB ½ 2 with rank = 4 (observable) and determinant not nearly equal to zero. Output matrix C ½ 3 produces an observability matrix OB ½ 3 with rank = 4 (observable) and determinant nearly equal to zero. The matrix condition number is very high indicating the system is barely observable. Output matrix C ½ 4 produces an observability matrix OB ½ 4 with rank = 3 (not observable) and determinant equal to zero with a matrix condition number equal to infinity. This means if all other states are measured by sensors, it is not possible to use an observer (even an optimal observer) to determine lateral deviation (cross-track error), y. It is a key state to measure with sensors. The sensor combinations that include y are observable. Using every other sensor, (except y) results in a system that is not observable. Furthermore, measuring y alone results in a barely observable system.
Assuming y is to be measured by a sensor, Table 4 reveals that measuring ν in addition to y produces the most observable system, and is recommended for designing reduced-order observers. The drawback is measuring ν requires a Doppler sonar, which may not always be available. If all states are measureable except ν the resulting reduced-order observer merely estimates ν using gains on the measureable states displayed in Table 5. Figure 10 reveals very good estimation of ν when all other states are sensed, and this estimated value of ν was fed to the motion controller in addition to the measured states (the poorly estimated states were neglected instead favoring the more-accurate measurements). State convergence to zero is achieved in the instance of state initialization 30 off-heading. Figure 11 displays similar results for the instance of state initialization one boat-length starboard.     Figure 12 compares the loop gains of the system with and without a compensator via gain margin and phase margin with full-state feedback, while Figure 13 displays the loop gains when output-feedback via observers is used. Each has relative strengths. Full state (theoretical) feedback yields infinite gain margin, yet relatively lower phase margin (usually consider more important of the two), while output feedback (real-world) yields good (but lesser) gain margin with increased phase margin.

Tracking systems and feedforward control in the presence of constant disturbance currents
This section evolves the earlier developed system equations and performance analysis by adding non-quiescent conditions, in particular introduction of a lateral underwater ocean current with an absolute velocity, υ 0 , requiring a modification of the system equations to add the lateral current to Eq. (4) resulting in Eq. (30).

Analysis of disturbed system in ocean currents via state equations and simulations
Using the controller (Eq. (19)) and the modified system equations where Eq. (4) is replaced by Eq. (30), and applying the final value theorem: f t ð Þ t!∞ sF s ð Þ s!0 , a steady state value 1/ω + 1 has some variable quantity added to unity for various υ 0 . Thus, steady-state errors exist in all cases with such disturbances, which are verified by simulations depicted in Figure 14 using gain values from the rule of thumb (ROT) for unity time constant. The steady-state errors are directly proportional to the disturbance magnitude. Figure 15 displays max rudder deflection for the maximal lateral ocean current in the study (to verify the control design continues to remain less than 0.4 radians) where we learned any current greater than 0.4 cannot be eliminated; therefore we next investigate feedforward control and integral control.

Elimination of steady-state error using feedforward control
Modify the control law to u f g feedforward ¼ δ f g ¼ ÀK 1 υ À K 2 r À K 3 ψ À K 4 y À K 0 in order to eliminate the steady-state error, where K 0 is chosen to insure zero steady-state error, where the feedback gains are chosen by the rule of thumb (Figures 16 and 17).   Autonomous Underwater Vehicle Guidance, Navigation, and Control 17

Disturbance estimation with reduced-order observer and integral control
Section 2.4 demonstrated feedforward control effectively countered the disturbance currents, but the current was presumed to be known. In to truly be effective, the reduced order observer is next augmented to include estimation of the unknown disturbance current velocityν c , where the observer now estimates the disturbance current velocity, the lateral sway velocity, ν, the lateral deviation (cross-track error), y, and the heading angle ψ. Figure 18a and b display the estimates of the unknown current for two current velocity conditions:ν c1 ¼ ν est1 ¼ 0:1and ν c2 ¼ ν est2 ¼ 0:5 respectively, while Figure 18c and d display the y and ψ states for each current velocity conditions. Notice how large rudder deflections modify the heading angle to the command-tracking value which counters the disturbance current (sometimes referred to as "crabbing"), and after establishing the crab heading angle, the rudder deflection goes towards zero, illustrating the effectiveness of command tracking. Figure 19 displays all the states versus time in seconds and also the trajectory when a worstcase unknown disturbance current ν c ¼ À0:5 is applied and estimated by the reduced-order observer where the observer gains are solutions to the linear quadratic Gaussian optimization. Meanwhile Figure 20 displays the results in cases utilizing command tracking with reduced order observer and with command: ψ = À0.5 and sinusoidal disturbance current υ c0 = Asin(0.1 t) but no disturbance estimation or feedforward, while Figure 21 uses disturbance estimation and feedforward and rule of thumb gains. Lastly, Figure 22 displays the performance of reduced-order observers, which is especially useful in instances of limited at-sea computational capabilities.

Waypoint guidance
A simple line-of-sight guidance routine was employed based on fixing waypoints through a minefield in order to navigate to a specified point and safely return home. The coordinates are   fed to a logic determining when to turn per Eq. (31), where d is the distance to the waypoint, and the heading command was autonomously calculated per Eq. (32).
Turnif : ffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffi ffi Particular attention is brought to the inverse tangent calculation, since quadrant must be preserved in the calculation, since the vehicle will navigate in 360 .

Results
The following paragraphs mirror Section 2. Above to provide a concise and precise description of the experimental results, their interpretation as well as the experimental conclusions that can be drawn in each sub-topic introduced and developed so far. Some new development naturally follows in the paragraphs of results, in response to the lessons learned.

System dynamics
Some basics lessons come from a brief analysis of the uncontrolled system dynamics. The open loop plant equations are potentially unstable (at least persistently oscillatory) with respect to only the bow rudder, while the relationship can be stable with respect to the stern rudder alone. Can be stable is exaggerated to emphasize the presence of pole-zero cancelation, which is an unwise practice (especially in this instance with both poles and zeros at the origin on the stability boundary) unless the estimates for the constants in the system equations are very well known. The analysis of the dynamics also revealed the bow rudder was least relativelyeffective at maneuvering alone when compared to the stern rudder, however the bow rudder does enhance vehicle maneuverability when used together with the stern rudder as a "scissored-pair" where the sign of the maneuver angle is opposite for each rudder. This "scissored-pair" constraint simplified the MIMO control design, allowing the design engineer to treat the system as a SISO design, since one rudder's deflection become a dependent variable constrained to the other rudder's deflection.

Control law design
Baseline proportional-derivative control designs effectively stabilized the dynamics, but were ineffective in the presence of a constant lateral open ocean current. Gains selected by rule of Figure 22. Utilization of command tracking with reduced order observer, with command: ψ = À0.5 and sinusoidal disturbance current ν c0 ¼ Asin 0:1t ð Þ, (a) with disturbance estimation (and feedforward), reduced order observer, (b) with integral control but no disturbance estimation or feedforward.
thumb performed similar to the linear-quadratic optimal control designs, so this underwater vehicle control could be designed at sea with rudimentary math in instances when higher level computational abilities are not available. Augmentation of the control in include gains tuned to reject the constant current proved effective, but required the current to be measure to permit the control component to be properly tuned. Furthermore, when the lateral disturbance current had sinusoidal variation, the controller was rendered ineffective rejecting the disturbance.

Observer design
The submersible vehicle's system equations were verified observable by calculation of a fullranked observability matrix in Section 2.3. A full sate observer was designed first to permit vehicle control with "full state feedback", yet without directly measuring velocity. Observer gains may be tuned using classical methods in the general spirit of duality between controller and observers. Their dual nature also permits the matrix Riccati equation to produce optimal gains for a linear-quadratic cost function that exclusively emphasizes state estimation error, unlike the controller optimization where the cost function balanced control effort with state error. State observers permit the vehicle operator to have smooth calculated estimates of all states at all times, which proves useful in the event of sensor interruptions or failures, and reduced-ordered observers may be used in instances where computations on-board the vehicle must be limited, for example to minimize computer size, weight, and/or power.
Especially in light of naturally occurring (roughly) sinusoidal variations in ocean current, the system equations were augmented to include the presumed-unknown disturbance as a state.

Tracking systems and feedforward control in the presence of disturbance currents
Simple feedforward control elements proved effective against known or estimated constant lateral disturbance currents by allowing the vehicle to autonomously perform "set-and-drift" principles where a highly trained helmsman would turn the bow of a ship into a current, but the simple feedforward elements were ineffective at countering currents with sinusoidal variation. In the set and drift principle the heading is de facto non-zero, so the vehicle cannot simultaneously maintain center-pointing while countering the disturbance. If such a requirement were added, designers must decouple the scissored-pair rudder constraint and design the rudder commands separately to simultaneously counter the disturbance while maintaining centerline pointing.

Disturbance estimation and integral control
Full-ordered observers effectively estimated constant and sinusoidal disturbance currents and proved useful in the control designs for feedforward control, but furthermore reduced-ordered observer were applied in cases where disturbances were forces and moments and feedforward control was not used. Integral control was used instead to drive steady-state error to zero where sufficiently large time-constants were used for the integrator, i.e. the fifth pole in the pole placement control must be less negative than the other poles.

Fully assembled system demonstration
In light of all these results, a fully assembled control system was used to navigate the proper mathematical models of the Phoenix autonomous submersible vehicle through a simulated 200 m Â 500 m minefield in the presence of unknown ocean currents. The field was populated randomly with 30+ mines, and vehicle successfully traversed the minefield in the presence of an unknown 0.5 m/s current with a miss distance from the nearest mine not less than 5 m, navigating from the starting point to pass within 0.5 m of a commanded en route point at sea, and then return to the start point. The outer loop controller used line-of-sight guidance to provide heading commands to the inner loop, and the inner loop controller was an outputfeedback heading controller. Two control strategies both proved effective: Linear-quadratic Gaussian, and approximate optimal pole-placement by rule of thumb. In the linear-quadratic Gaussian case, both the controller gains and observer gains were selected by optimization of the respective matrix Riccati equation. Figure 23 displays the completed maneuver where each dot displays the location of a randomly placed mine. Full state feedback was achieved with state observers via the certainly equivalence principle and the states were utilized in a proportional-derivative-integral feedback control architecture. Detailed outputs and figures of merit are plotted in Figures 24-28 including performance of a second transit of the minefield for validation purposes.

Discussion
The results of this study establish both classical and modern control paradigms to guide autonomous submersible vehicles through obstacles in unknown ocean currents. Both elegant and simplified autonomous controls proved effective, making this technology immediately assessable to low-end technology implementations. The results are consistent with the significant body of literature on motion mechanics in the presence of unknown disturbances with the added complication of restricted path planning due to randomly placed obstacles, where mines were used in this study driving an additional requirement of minimum safe distance for obstacle passage. This consistency with the current literature leads to a natural direction for future research, since recent innovations in nonlinear idealized (and sometimes also adaptive) methods have recently proven to be natural extensions of technology in these fields.  A natural sequel to this manuscript would utilize the aforementioned methods ( [26][27][28][29][30][31][32][33][34][35][36][37][38][39] in particular), which comprise nonlinear mathematical amplifications of the linear methods utilized here. The sequel should include an investigation of idealized nonlinear and adaptive methods with a direct comparison to the current state-of-the art including time-optimal control methods.