**1. Introduction**

The single most important near-term technical challenge of developing an autonomous capability for unmanned vehicles is to assess and respond appropriately to near-field objects in the path of travel. For unmanned aerial vehicles (UAVs), that near field may extend to several nautical miles in all directions, whereas for unmanned ground and maritime vehicles, the near field may only encompass a few dozen yards directly ahead of the vehicle. Nevertheless, when developing obstacle avoidance (OA) manoeuvres it is often necessary to implement a degree of deliberative planning beyond simply altering the vehicle's trajectory in a reactive fashion. For unmanned maritime vehicles (UMVs) the ability to generate near-optimal OA trajectories in real time is especially important when conducting sidescan sonar surveys in cluttered environments (e.g., a kelp forest or coral reef), operations in restricted waterways (e.g., rivers or harbours), or performing featurebased, terrain-relative navigation, to name a few. For example, a primary objective of sidescan sonar surveys is 100% area coverage while avoiding damage to the survey vehicle. Ideally, a real-time trajectory generator should minimize deviations from the preplanned survey geometry yet also allow the vehicle to retarget areas missed due to previous OA manoeuvres. Similarly, for operations in restricted waterways, effective OA trajectories should incorporate all known information about the environment including terrain, bathymetry, water currents, etc.

In the general case, this OA capability should be incorporated into an onboard planner or trajectory generator computing optimal (or near-optimal) feasible trajectories faster than in real time. For unmanned undersea vehicles (UUVs) the planner should be capable of generating full, three-dimensional (3D) trajectories, however some applications may require limiting the planner's output to two-dimensions (2D) for vertical-plane or horizontal-plane operating modes. For unmanned surface vehicles (USVs) the latter case is the only mode of operations.

Consider a typical hardware setup consisting of a UUV augmented with an autopilot (Fig.1). The autopilot not only stabilizes the overall system, but also enables vehicle control at a higher hierarchical level than simply changing a throttle setting ( ) *<sup>T</sup>*δ *t* , or deflecting stern plane ( ) *<sup>s</sup>* δ *t* or rudder ( ) *<sup>r</sup>* δ*t* angles.

In Fig.1, *WP* **x** , *WP* **y** , *WP* **z** are the vectors defining *x*, *y*, and *z* coordinates of some points in the local tangent (North-East-Down (NED)) plane for waypoint navigation. Alternatively a

Real-Time Optimal Guidance and Obstacle Avoidance for UMVs 69

sonar (FLS) systems employed for OA research at NPS. Section 6 addresses path-following considerations and practical implementation details for tracking nonlinear trajectories with conventional vehicle autopilots. Section 7 presents results from computer simulations and field experiments for several different scenarios which benefit from faster-than-real-time

Let us consider the most general case and formulate an optimization problem for computing collision-free trajectories in 3D (it can always be reduced to a 2D problem by eliminating two states). We will be searching within a set of admissible trajectories described by the state

where the components of the velocity vector – surge *u*, sway *v*, and heave *w*, defined in the body frame {*b*} – are added to the UUV NED coordinates *x*, *y* and *z* (0 *z* = at the surface and increases in magnitude with depth). While many UUVs are typically programmed to operate at a constant altitude above the ocean floor, it is still preferable to generate vertical trajectories in the NED local tangent plane because the water surface is a more reliable absolute reference datum than a possibly uneven sea floor. In general, however, it is a trivial matter to convert the resulting depth trajectory *z(t)* to an altitude trajectory *h(t)* for vehicles equipped with both altitude and depth sensors. Section 6.2 describes such practical

The admissible trajectories should satisfy the set of ordinary differential equations

() () () () () () *u b xt ut y t R vt zt wt* <sup>⎡</sup> ⎤ ⎡⎤ <sup>⎢</sup> ⎥ ⎢⎥ <sup>=</sup> <sup>⎢</sup> ⎥ ⎢⎥ <sup>⎢</sup> ⎥ ⎢⎥ <sup>⎣</sup> ⎦ ⎣⎦

*bR* is the rotation matrix from the body frame {*b*} to the NED frame {*u*}, defined using

cos ( )cos ( ) sin ( ) cos ( )sin ( ) ( ) sin ( )cos ( ) cos ( ) sin ( )sin ( )

*R t ψ t θ t ψ t ψ t θ t*

Although we are not going to exploit it in this study, admissible trajectories should also obey UUV dynamic equations describing translational and rotational motion. This means that the following linearized system holds for the vector **ς**(*t*), which includes speed

components *u*, *v*, *w* (being a part of our state vector **z**(*t*)) and angular rates *p*, *q*, *r*:

Here **A** and **B** are the state and control matrices and [,,]*<sup>T</sup>*

*ψ t θ t ψ t ψ t θ t*

sin ( ) 0 cos ( )

*θ t θ t* <sup>⎡</sup> <sup>−</sup> <sup>⎤</sup> <sup>⎢</sup> <sup>⎥</sup> <sup>=</sup> <sup>⎢</sup> <sup>⎥</sup> <sup>⎢</sup> <sup>−</sup> <sup>⎥</sup> <sup>⎣</sup> <sup>⎦</sup>

**ς**() () () *t tt* = + **Aς Bδ** (4)

*Tsr* **δ** = δ δ δ

(2)

(3)

is the control vector

 

two Euler angles, pitch *θ*( )*t* and yaw *ψ*( )*t* , and neglecting a roll angle as

*<sup>f</sup> t xt* <sup>=</sup> *<sup>y</sup> t zt ut vt wt S S t Z E t t t* ∈ = ∈⊂ ∈ <sup>⎡</sup> <sup>⎤</sup> <sup>⎣</sup> <sup>⎦</sup> **z z** (1)

[ ] { } 6 6 <sup>0</sup> ( ) ( ), ( ), ( ), ( ), ( ), ( ) , ( ) , , *<sup>T</sup>*

computation of near-optimal trajectories.

**2. Problem formulation** 

considerations in detail.

describing the UUV kinematics

*u b*

vector

In (2) *<sup>u</sup>*

(Healey, 2004).

typical autopilot may also accept some reference flight-path angle γ ( )*t* (or altitude/depth) command and heading Ψ( )*t* (or yaw angle ψ( )*t* ), respectively. The motion sensors, accelerometers, and rate gyros measure the components of inertial acceleration, ( ) *<sup>I</sup> x t* , ( ) *<sup>I</sup> y t* and ( ) *<sup>I</sup> z t* , and angular velocity – roll rate *p*( )*t* , pitch rate *q t*( ), and yaw rate *r t*( ) .

Fig. 1. A. UUV augmented with an autopilot

A trajectory generator would consider an augmented UUV as a new plant (Fig.2) and provide this plant with the necessary inputs based on the mission objectives (final destination, time of arrival, measure of performance, etc.). Moreover, the reference signals, γ ( )*t* and Ψ( )*t* , are to be computed dynamically (once every few seconds) to account for disturbances (currents, etc.) and newly detected obstacles.

Fig. 2. Providing an augmented UUV with a reference trajectory

Ideally, the trajectory generator software should also produce the control inputs ( ) *ref* **δ** *t* corresponding to the feasible reference trajectory (Fig.3) (Basset et al., 2008). This enhanced setup assures that the inner-loop controller deals only with small errors. (Of course this setup is only viable if the autopilot accepts these direct actuator inputs.)

Fig. 3. Providing an augmented UUV with the reference trajectory and reference controls

The goal of this chapter is to present the dynamic trajectory generator developed at the Naval Postgraduate School (NPS) for the UMVs of the Center for Autonomous Vehicle Research (CAVR) and show how the OA framework is built upon it. Specifically, Section 2 formulates a general feasible trajectory generation problem, followed by Section 3, which introduces the general ideas behind the proposed framework for solving this problem that utilizes the inverse dynamics in the virtual domain (IDVD) method. Section 4 considers simplifications that follow from reducing the general spatial problem to two planar subcases. Section 5 describes the REMUS UUV and SeaFox USV and their forward looking sonar (FLS) systems employed for OA research at NPS. Section 6 addresses path-following considerations and practical implementation details for tracking nonlinear trajectories with conventional vehicle autopilots. Section 7 presents results from computer simulations and field experiments for several different scenarios which benefit from faster-than-real-time computation of near-optimal trajectories.
