**6.1 Horizontal plane**

84 Autonomous Underwater Vehicles

estimate for the occupancy state of each cell can be continually updated using an iterative technique that incorporates all previous measurements (Elfes, 1989). Figure 16a shows an occupancy grid map of a river generated by the SeaFox FLS system. In this image, each pixel corresponds to a 1-metre square grid cell whose colour represents the cell's probability of being occupied (red) or empty (green). For comparison, the inset portion of the occupancy grid map has been overlaid with an obstacle map of oriented bounding boxes in Fig.16b. Clearly, using discrete bounding boxes to represent a long, continuous shoreline quickly becomes intractable as more and more boxes are required. The occupancy grid framework is a much more efficient obstacle map representation for wide area operations in restricted

waterways.

a) b)

of collision along the entire trajectory.

**6. Path following** 

Fig. 16. Occupancy grid for a river as generated by the SeaFox FLS system

NPS has developed probabilistic sonar models for the BlueView FLS and has successfully combined separate 2D occupancy grids in order to reconstruct the 3D geometry of an obstacle imaged by the REMUS UUV's horizontal and vertical sonar arrays (Horner et al., 2009). Using this occupancy grid framework, each candidate trajectory's risk of obstacle collision is computed using the occupancy probabilities (a direct lookup operation) of the grid cells it traverses. Trajectory optimization for OA entails minimizing the cumulative risk

While the REMUS UUV and SeaFox USV are both commercial vehicles with proprietary autopilots, both provide communications interfaces that allow experimental sensor and computer payloads to override the primary autopilot via high-level commands. The REMUS RECON interface, for example, closely resembles the augmented autopilot depicted in Figs. 2 and 3 (although direct actuator inputs are only available for propeller and cross-body thrusters settings). For full overriding control, a payload module must periodically send valid commands containing all of the following: i) desired depth or altitude, ii) desired vehicle or propeller speed, and iii) desired heading, turn rate, or waypoint location. The developed trajectory generator (described in Section 3) outputs reference trajectories as parameterized expressions for each coordinate in a spatial curve plus a speed factor to use while traversing that curve. Using these expressions as reference trajectories, the 3D path following controller developed earlier (Kaminer et al., 2007) can compute turn rate and pitch rate commands required to drive a vehicle onto (and along) the desired trajectory. The Consider the 2D problem geometry depicted in Fig.17, which defines an inertial {*I*} frame, Serret-Frenet {*F*} error frame and body-fixed reference frame {*b*}. The kinematic model of the vehicle (2)-(3) reduces to

$$
\begin{bmatrix}
\dot{x}(t) \\
\dot{y}(t)
\end{bmatrix} = \begin{bmatrix}
\mathcal{U}\_0 \cos \psi(t) \\
\mathcal{U}\_0 \sin \psi(t)
\end{bmatrix} \tag{33}
$$

with dynamics described by

$$
\psi = r \tag{34}
$$

Fig. 17. Horizontal path-following kinematics

By construction, the local trajectory planner produces an analytic expression for each component of the spatial trajectory as a function of virtual arc length, ( ) *<sup>c</sup>* **p** τ . We can also compute analytic expressions for ( ) *<sup>c</sup>* **p**′ τ and ( ) *<sup>c</sup>* **p**′′ τ , the first and second derivatives, respectively, of the spatial trajectory. Using the relationships in Fig.17, the errors can be expressed in the Serret-Frenet frame {*F*} as

$$\mathbf{q}\_F = \begin{bmatrix} \mathbf{x}\_F \\ \mathbf{y}\_F \end{bmatrix} = \,^F\_I \mathbf{R} (\mathbf{q}\_I - \mathbf{p}\_c) \tag{35}$$

where [ , , ] *F T <sup>I</sup> R* = **TNB** is a rotation matrix constructed from the tangent, normal, and binormal vectors of the Serret-Frenet error frame {*F*}. The tangent vector is computed from the expression for the trajectory's first derivative as:

Real-Time Optimal Guidance and Obstacle Avoidance for UMVs 87

Now consider the REMUS UUV manoeuvring in the vertical plane using altitude commands. For survey operations, REMUS is typically programmed to follow a lawnmower pattern at a constant altitude above the sea floor determined by the desired sidescan sonar range. Since the ADCP/DVL sensor continuously measures vehicle altitude above the bottom, this operating mode ensures safe operation over undulating bottoms with slopes of up to 45 degrees (Healey, 2004). Obstacle avoidance manoeuvres are required to safely negotiate steeper slopes, step-like terrain features (e.g. sand bars or coral heads), or objects proud of the ocean floor. As described earlier, since the REMUS FLS is mounted in a fixed orientation, it may detect new obstacles while executing a manoeuvre to avoid the current obstacle threat. Periodic or detection-based replanning can handle these situations. This

When negotiating a step-up ridge or sand bar whose extent is not known due to sonar occlusion at the time of detection, it may not be desirable to follow the planned vertical trajectory to its completion. Between planning iterations, a simple yet safer approach is to revert back to constant altitude control once the vehicle reaches a position directly above the detected object boundaries. This condition can be checked using a 2.5D version of the 3D bounding box intersection test described above. Figure 18 illustrates a simulation whereby the REMUS FLS detects the leading edge of a ridge at maximum sonar range. Image processing algorithms compute range to the object (80m) as well as its width (W, into the page) and its height above the seafloor (5.5m) but cannot determine the length of the ridge since it is occluded by its own leading edge. Therefore, the obstacle detection algorithm generates a 3D bounding box measuring W m wide x 1.0m long (assumed) x

While the IDVD-method planner generates a vertical trajectory in NED coordinates, in shallow water it is safer to operate the vehicle in an altitude control mode. Therefore, the vertical coordinate trajectory is converted from a depth plan into an altitude plan by assuming constant water depth over the planning horizon and using the

(0) ( ) ( )

Δ= − = +Δ (42)

is the altitude command sent to the autopilot via the RECON

 τ

> τ

*plan h* is shown in Fig.18 as a deviation from the

τ

() () () ( ) *nom plan plan plan plan nom cmd nom plan*

*Dh z h Dz h h hh h h*

= + = −

ττ

where *D* is the water depth computed at planner initialization time, *nom h* is the nominal

τ

nominal mission segment altitude. As seen, sending this altitude command directly to the vehicle autopilot would cause an undesirable jump in the altitude profile once the ADCP/DVL sensor measures the vehicle's true altitude above the ridge. Instead, switching the altitude command to the nominal mission segment altitude once the vehicle reaches the ridge will produce the desired altitude profile). Note that even though Fig.18 depicts a sudden drop in altitude when the vehicle passes the ridge while commanding the nominal mission segment altitude, in practice the vehicle dynamics will ensure that the UUV

τ

τ

executes a smooth transition back to its nominal survey altitude.

interface. The resulting altitude plan Δ ( )

**6.2 Vertical plane** 

5.5m high.

relationships

altitude set point, and ( ) *cmd h*

scenario was illustrated conceptually in Fig.7.

$$\mathbf{T} = \frac{\mathbf{p}\_c'(\boldsymbol{\tau})}{\|\mathbf{p}\_c'(\boldsymbol{\tau})\|}\tag{36}$$

For the 2D problem, the normal vector components can be computed directly from the tangent vector components: *N T <sup>x</sup>* = − *<sup>y</sup>* and *N T <sup>y</sup>* = *<sup>x</sup>* . Additionally, the signed curvature of the trajectory can be computed using the expressions for the trajectory's first and second derivatives as:

$$\kappa(\tau) = \frac{p\_{cy}''(\tau)p\_{cx}'(\tau) - p\_{cx}''(\tau)p\_{cy}'(\tau)}{\left\| \mathbf{p}\_c'(\tau) \right\|^3} \tag{37}$$

Taking the time derivative of **q***<sup>F</sup>* , we obtain the following state space representation for the error kinematics (i.e. the position and heading of the vehicle's body-fixed frame {*b*} relative to the Serret-Frenet frame {*F*}, which follows the desired trajectory):

$$\begin{aligned} \dot{\mathbf{x}}\_F &= -\dot{l}(\mathbf{1} - \kappa \mathbf{y}\_F) + \mathbf{U}\_0 \cos \boldsymbol{\nu}\_e \\ \dot{\mathbf{y}}\_F &= -\dot{l}(\kappa \mathbf{x}\_F) + \mathbf{U}\_0 \sin \boldsymbol{\nu}\_e \\ \dot{\boldsymbol{\nu}}\_e &= \dot{\boldsymbol{\nu}} - \dot{\boldsymbol{\nu}}\_F = \boldsymbol{\nu}\_\nu - \dot{\boldsymbol{\nu}}\_F = \boldsymbol{\nu}\_\nu - \kappa \dot{\boldsymbol{l}} \end{aligned} \tag{38}$$

where *l* is the path length of the desired spatial curve and *l* describes the speed at which a virtual target travels along this curve.

The goal is to drive the vehicle's position error ( **q***<sup>F</sup>* ) and heading error (Ψ*<sup>e</sup>*) to zero. This will drive the vehicle to the commanded trajectory location ( **p***<sup>c</sup>* ) and align its velocity vector with the trajectory's tangent vector (**T**). The control signal *u*ψ must now be chosen to asymptotically drive the vehicle position and velocity vectors onto the commanded trajectory. We choose the candidate Lyapunov function

$$V = \frac{1}{2} \left( \mathbf{x}\_F^2 + y\_F^2 + (\boldsymbol{\nu}\_e - \boldsymbol{\delta}\_\nu)^2 \right) \tag{39}$$

where ψ δ is a shaping function that controls the manner in which the vehicle approaches the path

$$\mathcal{S}\_{\mathcal{Y}} = \sin^{-1}\left(\frac{-y\_F}{\left|y\_F\right| + d}\right) \tag{40}$$

with 0 *d* > an arbitrary constant.

Using some algebra, we choose the following control laws to ensure that *V* < 0 :

$$\begin{aligned} \dot{l} &= K\_1 \mathbb{x}\_F + \mathcal{U}\_0 \cos \psi\_\varepsilon \\ \mu\_\nu &= \kappa \dot{l} + \dot{\mathcal{S}}\_\nu + K\_2 (\mu\_\varepsilon - \mathcal{S}\_\nu) - \frac{\sin \psi\_\varepsilon - \sin \delta\_\psi}{\nu\_\varepsilon - \mathcal{S}\_\nu} \mathcal{U}\_0 \mu\_F \end{aligned} \tag{41}$$

In these expressions *K*<sup>1</sup> , *K*<sup>2</sup> , and *d* can be used as gains to tune the closed-loop performance of the path following controller.

### **6.2 Vertical plane**

86 Autonomous Underwater Vehicles

′ = ′ **<sup>p</sup> <sup>T</sup> p**

For the 2D problem, the normal vector components can be computed directly from the tangent vector components: *N T <sup>x</sup>* = − *<sup>y</sup>* and *N T <sup>y</sup>* = *<sup>x</sup>* . Additionally, the signed curvature of the trajectory can be computed using the expressions for the trajectory's first and second

> () () () () ( ) ( ) *cy cx cx cy c*

′′ ′ ′′ ′ <sup>−</sup> <sup>=</sup>

Taking the time derivative of **q***<sup>F</sup>* , we obtain the following state space representation for the error kinematics (i.e. the position and heading of the vehicle's body-fixed frame {*b*} relative

τ

τ

*x l yU y lx U*

=− − + =− +

κ

ψψ

The goal is to drive the vehicle's position error ( **q***<sup>F</sup>* ) and heading error (

κ

κ τ

to the Serret-Frenet frame {*F*}, which follows the desired trajectory):

 

ψ

with the trajectory's tangent vector (**T**). The control signal *u*

trajectory. We choose the candidate Lyapunov function

where *l* is the path length of the desired spatial curve and *l*

virtual target travels along this curve.

*pp pp*

derivatives as:

where

the path

ψ δ

with 0 *d* > an arbitrary constant.

( ) ( ) *c c* τ

(36)

**p**′ (37)

(38)

describes the speed at which a

(39)

(40)

*<sup>e</sup>*) to zero. This will

must now be chosen to

Ψ

τ

3

τ

0

 ψ

*u ul*

 ψ  κ

ψ

 ψ

0

 ψ

(1 ) cos ( ) sin *FFe FF e eF F*

ψ

drive the vehicle to the commanded trajectory location ( **p***<sup>c</sup>* ) and align its velocity vector

asymptotically drive the vehicle position and velocity vectors onto the commanded

( ) <sup>1</sup> 22 2 ( ) <sup>2</sup> *V xy* = ++ − *FF e* ψ δψ

> <sup>1</sup> sin *<sup>F</sup> F y y d*

<sup>−</sup> ⎛ ⎞ <sup>−</sup> <sup>=</sup> ⎜ ⎟

ψδ

Using some algebra, we choose the following control laws to ensure that *V* < 0 :

cos

 ψδ

ψ

<sup>−</sup> =+ + − − <sup>−</sup>

*F e*

( )

*ul K U y*

 ψ

In these expressions *K*<sup>1</sup> , *K*<sup>2</sup> , and *d* can be used as gains to tune the closed-loop

1 0

*l Kx U*

= +

performance of the path following controller.

κδ

ψψ

is a shaping function that controls the manner in which the vehicle approaches

+ ⎝ ⎠

2 0

sin sin

ψ

 δ

(41)

ψ

*e e F e*

ψ

ψ δ

= −=−=−

 ττ Now consider the REMUS UUV manoeuvring in the vertical plane using altitude commands. For survey operations, REMUS is typically programmed to follow a lawnmower pattern at a constant altitude above the sea floor determined by the desired sidescan sonar range. Since the ADCP/DVL sensor continuously measures vehicle altitude above the bottom, this operating mode ensures safe operation over undulating bottoms with slopes of up to 45 degrees (Healey, 2004). Obstacle avoidance manoeuvres are required to safely negotiate steeper slopes, step-like terrain features (e.g. sand bars or coral heads), or objects proud of the ocean floor. As described earlier, since the REMUS FLS is mounted in a fixed orientation, it may detect new obstacles while executing a manoeuvre to avoid the current obstacle threat. Periodic or detection-based replanning can handle these situations. This scenario was illustrated conceptually in Fig.7.

When negotiating a step-up ridge or sand bar whose extent is not known due to sonar occlusion at the time of detection, it may not be desirable to follow the planned vertical trajectory to its completion. Between planning iterations, a simple yet safer approach is to revert back to constant altitude control once the vehicle reaches a position directly above the detected object boundaries. This condition can be checked using a 2.5D version of the 3D bounding box intersection test described above. Figure 18 illustrates a simulation whereby the REMUS FLS detects the leading edge of a ridge at maximum sonar range. Image processing algorithms compute range to the object (80m) as well as its width (W, into the page) and its height above the seafloor (5.5m) but cannot determine the length of the ridge since it is occluded by its own leading edge. Therefore, the obstacle detection algorithm generates a 3D bounding box measuring W m wide x 1.0m long (assumed) x 5.5m high.

While the IDVD-method planner generates a vertical trajectory in NED coordinates, in shallow water it is safer to operate the vehicle in an altitude control mode. Therefore, the vertical coordinate trajectory is converted from a depth plan into an altitude plan by assuming constant water depth over the planning horizon and using the relationships

$$\begin{aligned} D &= h\_{\text{nom}} + z(0) & h\_{\text{plan}}(\tau) &= D - z\_{\text{plan}}(\tau) \\ \Delta h\_{\text{plan}}(\tau) &= h\_{\text{plan}}(\tau) - h\_{\text{nom}} & h\_{\text{cmd}}(\tau) &= h\_{\text{nom}} + \Delta h\_{\text{plan}}(\tau) \end{aligned} \tag{42}$$

where *D* is the water depth computed at planner initialization time, *nom h* is the nominal altitude set point, and ( ) *cmd h* τ is the altitude command sent to the autopilot via the RECON interface. The resulting altitude plan Δ ( ) τ *plan h* is shown in Fig.18 as a deviation from the nominal mission segment altitude. As seen, sending this altitude command directly to the vehicle autopilot would cause an undesirable jump in the altitude profile once the ADCP/DVL sensor measures the vehicle's true altitude above the ridge. Instead, switching the altitude command to the nominal mission segment altitude once the vehicle reaches the ridge will produce the desired altitude profile). Note that even though Fig.18 depicts a sudden drop in altitude when the vehicle passes the ridge while commanding the nominal mission segment altitude, in practice the vehicle dynamics will ensure that the UUV executes a smooth transition back to its nominal survey altitude.

Real-Time Optimal Guidance and Obstacle Avoidance for UMVs 89

geometry of the MURS allows the planner to construct a "keep out" zone corresponding to the MURS propeller and aft control surfaces. The UUV rendezvous trajectory must avoid this area. Once the rendezvous plan has been agreed upon and acknowledged, both the UUV and the MURS proceed to position 3 for rendezvous (stage B). Finally, at position 4 the

The simulated rendezvous scenario assumes three stages: communication (A), execution (B), and recovery (C), respectively. From the trajectory generation standpoint we are primarily concerned with optimizing the path that would bring the UUV from its current position (point 2) to a certain rendezvous state (point 3) in the preset time *Tr* proposed by the MURS, while obeying all possible real-life constraints and avoiding the MURS keep out zone. Figures 21 and 22 present a computer simulation in which a MURS is moving due east at 1*m*/*s* (1.94*kn*) with the docking station at a depth of 15m. A UUV is located 800 meters away. The MURS wishes to conduct a rendezvous operation *Tr* minutes later and sends the corresponding information to the UUV. This information includes the proposed final position *<sup>f</sup> x* , *<sup>f</sup> y* , *<sup>f</sup> z* rendezvous course, speed, and time. Figure 21 shows several generated trajectories, which meet the desired objectives for this scenario and also avoid an obstacle located along the desired path to MURS. These trajectories differ by the arrival time *Tr* . During handshaking communications with the MURS, the UUV determines whether the suggested *Tr* is feasible. Of the four trajectories shown, the trajectory generated for

recovery operation (stage C) is completed.

Fig. 19. Manifold of initial and final conditions

Fig. 20. Proposed rendezvous scenario

Fig. 18. Simulation results for vertical OA using UUV altitude control mode
