Comparison of simulation integration methodology.

## Abstract

A considerable volume of research has recently blossomed in the literature on autonomous 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.

### Keywords

- submersible vehicles
- ocean research
- obstacle avoidance
- guidance
- navigation
- and control
- linear quadratic optimal control
- approximated optimal control
- reduced-order observers
- MIMO
- SIMO

## 1. 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 high-confidence mathematically modeling in computer simulations. Such simulations permitted Wu et al. [11] to redesign the L1 adaptive control architecture for pitch-control with anti-windup 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 single-channel 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.

## 2. 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.

### 2.1. 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′) 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).

where

The variable are | The constants are | ||

ν | lateral (sway) velocity | m = 0.0358 | |

r | turning rate (yaw) | I_{z} = 0.0022 | |

ψ | heading angle (degrees) | x_{G} = 0.0014 | |

y | lateral deviation (cross-track error) | ||

δ_{s} | stern rudder deflection | ||

δ_{b} | bow rudder deflection | ||

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)

*, position of the vehicle’s center of gravity (measured positive forward of amidships)*I

_{z}

*, with the remaining terms referred to as the hydrodynamic coefficients. These constants are all presented in non-dimensional form.*x

_{G}

Defining the state vector

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 investigated (

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.

Rudder deflected | Euler: -distance | Runge-Kutta: -distance | Euler: -distance | Runge-Kutta: -distance |
---|---|---|---|---|

Bow | 6.5471 | 6.5469 | 6.8647 | 6.8646 |

Stern | 3.1665 | 3.1665 | 3.5768 | 3.5768 |

Both | 2.4546 | 2.4546 | 2.6567 | 2.6567 |

### 2.2. 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 scissored-pair the transfer function from rudder input to output

*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 ([*y

*] and [*CO

*] respectively) are listed in Eq. (15) (whose matrix product [*OB

*] 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.*OC

Diagonalizing the original system [* A*] matrix, the spectral decomposition

normal-coordinate form

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.

* to roughly locate closed-loop poles per*Eq. (20)

. Then place other poles at slightly different locations (e.g.

)

The gains achieved using the rule of thumb K_{R.O.T.} = {0.5070 –0.3687 -0.7157 -0.1972} have quite different values compared to the gains calculated through the matrix Riccati equation in the linear-quadratic optimization K_{LQR} = {−0.0939 –1.2043 -2.2138 -1}, but nonetheless the resulting behaviors are indeed very similar.

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

*converges to zero along with the other states. Furthermore, the results of rudder-limited simulations are displayed in Figure 6 and Figure 7 for both scenarios (Table 2).*y

Time constant | ||||
---|---|---|---|---|

0.5 | −1.5135 | −1.7005 | −5.1508 | −3.22524 |

1 | 0.5070 | −0.3687 | −0.7157 | −0.1972 |

2 | 1.1248 | 0.2870 | −0.0906 | −0.0116 |

LQR | −0.0939 | −1.2043 | −2.2138 | −1 |

### 2.3. Observer design

To design a state observer, the system must be observable [4], verifiable through examination of the observability matrix [* OB*] per Eq. (21), where [

*] = [*C

*] = [0 1 1 1]. The condition of the observability matrix reveals the degree of observability, and it is defined by the ratio of maximum to minimal singular values.*ν r ψ y

#### 2.3.1. Full-order observer design

Assuming that only * rule of thumb*relationship between gains and time constant as done for the controller gains (Table 3).

Multiple of controller time constant used for observer | Observer gain matrix |
---|---|

Figure 8 displays the results of simulations revealing the accuracy of state estimation when * rule of thumb*, where the time constant is chosen to be half (

*= 1/2) the time constant of the controller (*t

_{c}

*= 1) and the simulation is initialized with the heading angle 30° off, while Figure 9 displays the simulation initialized one boat-length starboard position.*t

_{c}

#### 2.3.2. 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 * y*. It is a key state to measure with sensors. The sensor combinations that include

*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.*y

Assuming

Sensors used to measure states | Observability matrix condition number |
---|---|

8.8456 | |

21.1306 | |

31.2919 |

Multiple of controller time constant used for observer | Observer gain matrix |
---|---|

#### 2.3.3. Gain margin and phase margin

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.

### 2.4. 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,

#### 2.4.1. 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: * 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.

#### 2.4.2. Elimination of steady-state error using feedforward control

Modify the control law to * rule of thumb*(Figures 16 and 17).

### 2.5. 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

Figure 19 displays all the states versus time in seconds and also the trajectory when a worst-case unknown disturbance current

### 2.6. 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).

Particular attention is brought to the inverse tangent calculation, since quadrant must be preserved in the calculation, since the vehicle will navigate in 360°.

## 3. 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.

### 3.1. 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 relatively-effective 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.

### 3.2. 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 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.

### 3.3. Observer design

The submersible vehicle’s system equations were verified observable by calculation of a full-ranked 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.

### 3.4. 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.

### 3.5. 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.

### 3.6. 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 output-feedback heading controller. Two control strategies both proved effective: Linear-quadratic Gaussian, and approximate optimal pole-placement by

*. 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.*rule of thumb

## 4. 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.

## Acknowledgments

Conceptualization by Distinguished Professor Healey foremost, and then Professor Kevin Bollino and Sands afterwards; methodology by Healey; Software by Bollino and Sands; validation by Professor Isaac Kaminer, Sands, and Bollino; formal analysis by Sands and Bollino; writing-original draft preparation by Sands; writing-review & editing by Sands; research supervision by Kaminer. Authorship has been limited to those who have contributed substantially to the work reported. * Professor Sands is not a tenured faculty member of Stanford University*; rather he is the Associate Dean of the Naval Postgraduate School’s Graduate School of Engineering and Applied Sciences. In order to avoid legal jeopardy, Dr. Sands publishes government-funded research under his association with the Naval Postgraduate School, while publishing non-government funded research under his continuing associations with Stanford and Columbia Universities.