**2. Problem formulation**

68 Autonomous Underwater Vehicles

accelerometers, and rate gyros measure the components of inertial acceleration, ( ) *<sup>I</sup>*

*z t* , and angular velocity – roll rate *p*( )*t* , pitch rate *q t*( ), and yaw rate *r t*( ) .

ψ

**Sensors**

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

*<sup>x</sup>*( ), ( ), ( ) *t yt zt* **Dynamic Trajectory** 

*<sup>x</sup>*( ), ( ), ( ) *t yt zt* **Dynamic Trajectory** 

() , () *ref ref*

 *t t* Ψ () , () *ref ref zt t* ψ

Sensor Data Position Estimate

() , () *ref ref*

 *t t* Ψ () , () *ref ref zt t* ψ

Sensor Data Position Estimate 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

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

γ

γ

**Vehicle**

( ), ( ), ( ) ( ), ( ), ( ) *I II x t ytzt p t qt rt* 

**Augmented Vehicle**

**Augmented Vehicle (with Sensors and Controller)**

**Augmented Vehicle (with Sensors and Controller)**

( )*t* ), respectively. The motion sensors,

*<sup>x</sup>*( ), ( ), ( ) *t yt zt* **<sup>δ</sup>**( )*<sup>t</sup>*

( )*t* (or altitude/depth)

*x t* , ( ) *<sup>I</sup> y t*

typical autopilot may also accept some reference flight-path angle

command and heading Ψ( )*t* (or yaw angle

**Autopilot**

Fig. 1. A. UUV augmented with an autopilot

**Reference Signal Generator**

disturbances (currents, etc.) and newly detected obstacles.

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

( ) *ref* **δ** *t*

**Generator**

**Generator**

( ), ( ) *t t* Ψ **Controller**

and ( ) *<sup>I</sup>*

γ

γ

Mission goals

Mission goals

*zt t* ( ), ( ) ψ

, , *WP WP WP* **xyz**

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 vector

$$\mathbf{z}(\mathbf{z}(t) = \left[\mathbf{x}(t), y(t), z(t), u(t), v(t), w(t)\right]^T \in \mathcal{S} \,, \ S = \left\{\mathbf{z}(t) \in \boldsymbol{Z}^6 \subset \boldsymbol{E}^6\right\} \,, \ t \in \left[t\_0, t\_f\right] \tag{1}$$

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 considerations in detail.

The admissible trajectories should satisfy the set of ordinary differential equations describing the UUV kinematics

$$\begin{bmatrix} \dot{x}(t) \\ \dot{y}(t) \\ \dot{z}(t) \end{bmatrix} = \,^\mu\_\nu R \begin{bmatrix} u(t) \\ v(t) \\ w(t) \end{bmatrix} \tag{2}$$

In (2) *<sup>u</sup> bR* is the rotation matrix from the body frame {*b*} to the NED frame {*u*}, defined using two Euler angles, pitch *θ*( )*t* and yaw *ψ*( )*t* , and neglecting a roll angle as

$${}^{\mu}\_{b}R(t) = \begin{bmatrix} \cos\psi(t)\cos\theta(t) & -\sin\psi(t) & \cos\psi(t)\sin\theta(t) \\ \sin\psi(t)\cos\theta(t) & \cos\psi(t) & \sin\psi(t)\sin\theta(t) \\ -\sin\theta(t) & 0 & \cos\theta(t) \end{bmatrix} \tag{3}$$

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*:

$$
\dot{\mathbf{q}}(t) = \mathbf{A}\mathbf{q}(t) + \mathbf{B}\mathbf{\hat{o}}(t) \tag{4}
$$

Here **A** and **B** are the state and control matrices and [,,]*<sup>T</sup> Tsr* **δ** = δ δ δ is the control vector (Healey, 2004).

Real-Time Optimal Guidance and Obstacle Avoidance for UMVs 71

several important properties required for real-time implementations: i) the boundary conditions including high-order derivatives are satisfied *a priori*; ii) the resulting control commands are smooth and physically realizable, iii) the method is very robust and is not sensitive to small variations in input parameters, iv) any compound performance index can be used during optimization. Moreover, this specific method uses only a few variable parameters, thus ensuring that the iterative process during optimization converges very fast compared to other direct methods. The IDVD-based trajectory generator consists of several blocks. The goal of the first block, to be discussed next, is to produce a candidate trajectory,

Again, consider the most general case of a UUV operating in 3D (as opposed to a USV). Suppose that each coordinate *<sup>i</sup> x* , *i* = 1,2,3 of the candidate UUV trajectory is represented as

0

*M*

analytic expressions for the trajectory coordinates can be constructed from any combination of basis functions to produce a rich variety of candidate trajectories. For example, a combination of monomials and trigonometric functions was utilized in (Yakimenko &

As discussed in (Yakimenko, 2000; Horner & Yakimenko, 2007) the degree *M* is determined by the number of boundary conditions that must be satisfied. Specifically, it should be greater or equal to the number of preset boundary conditions but one. In general the desired trajectory includes constraints on the initial and final position, velocity and acceleration: *<sup>i</sup>*<sup>0</sup> *x* , *if x* , *<sup>i</sup>*<sup>0</sup> *x*′ , *if x*′ , *<sup>i</sup>*<sup>0</sup> *x*′′ , *if x*′′ . In this case the minimal order of polynomials (9) is 5, because all coefficients in (9) will be uniquely defined by these boundary conditions, leaving the

candidate trajectory, additional varied parameters can be obtained by increasing the order of the polynomials (9). For instance, using seventh-order polynomials will introduce two more varied parameters for each coordinate expression. Rather than varying two coefficients in these extended polynomials directly, we will vary the initial and final jerk, *<sup>i</sup>*<sup>0</sup> *x*′′′ and *if x*′′′ , respectively. In this case, coefficients *ik a* in (9) can be determined by solving the obvious system of linear algebraic equations equating polynomials (9) to *<sup>i</sup>*<sup>0</sup> *x* , *if x* , *<sup>i</sup>*<sup>0</sup> *x*′ , *if x*′ , *<sup>i</sup>*<sup>0</sup> *x*′′ , *if x*′′ ,

By construction, the boundary conditions (5) will be satisfied unconditionally for any value

Figure 4 demonstrates a simple example whereby a UUV operating 2m above the sea floor at 1.5m/s must perform a pop-up manoeuvre to avoid some obstacle. Even with a single

heights. Similar trajectories could be produced solely in the horizontal plane or in all three dimensions. It should be pointed out that even at this stage infeasible candidate trajectories

*i ik k x a* τ

( )

τ ≡ *x* τ τ

 , <sup>2</sup> *x* () () τ ≡ *y* τ

*k*

τ =

, the virtual arc

as the only varied parameter. For more flexibility in the

) (Yakimenko, 2000, 2008).

will alter the shape of the candidate trajectory.

allows the UUV to avoid obstacles of different

<sup>=</sup> ∑ , (9)

 and <sup>3</sup> *x* () () τ ≡ *z* τ

). In general,

which satisfies the boundary conditions.

**3.1 Generating a candidate trajectory** 

a polynomial of degree *M* of some abstract argument

(for simplicity of notation we assume <sup>1</sup> *x* () ()

τ

τ

. However, varying *<sup>f</sup>*

 = 0 and *<sup>f</sup>* τ =τ

τ

τ

Slegers, 2009).

"length" of the virtual arc *<sup>f</sup>*

*<sup>i</sup>*<sup>0</sup> *x*′′′ and *if x*′′′ at two endpoints (

τ

varied parameter, changing the value of *<sup>f</sup>*

of the final arc *<sup>f</sup>*

Next, the admissible trajectories (1) should satisfy the initial and terminal conditions

$$\mathbf{z}(t\_0) = \mathbf{z}\_0 \; , \; \mathbf{z}(t\_f) = \mathbf{z}\_f \tag{5}$$

Finally, certain constraints should be obeyed by the state variables, controls and their derivatives. For example, in the case of a UUV these can include obvious constraints on the UUV depth:

$$z\_{\text{min}} \le z(t) \le z\_{\text{max}} \tag{6}$$

where max *z xy* (,) describes a programmed operational depth limit. For vehicles programmed to operate at some nominal altitude above the sea floor, the max *z xy* (,) constraint can be converted into a minimum altitude min *h xy* (,) constraint as described in Section 6.2.

A 3D OA requirement can be formulated as

$$[\mathbf{x}(t); y(t); \mathbf{z}(t)] \cap \mathfrak{R} = \mathbf{0} \tag{7}$$

where ℜ is the set of all known obstacle locations. The constraints are usually imposed not only on the controls themselves **δ δ** <sup>≤</sup> max but on their time derivatives as well **δ δ** <sup>≤</sup> max � � to account for actuator dynamics. Knowing the system's dynamics (4) (or simply complying with the autopilot specifications), these latter constraints can be elevated to the level of the reference signals, for instance

$$\left|\theta(t)\right| \le \theta\_{\text{max}} \quad \text{and} \quad \left|\dot{\varphi}(t)\right| \le \dot{\varphi}\_{\text{max}} \tag{8}$$

The objective is to find the best trajectory and corresponding control inputs that minimize some performance index *J*. Typical performance index specifications include: i) minimizing time of the manoeuvre *<sup>f</sup>* <sup>0</sup> *t t* − , ii) minimizing the distance travelled to avoid the obstacle(s), and iii) minimizing control effort or energy consumption. In addition, the performance index may include some "exotic" constraints dictated by a sensor payload. For example, a UUV may require vehicle trajectories which point a fixed FLS at specified terrain features or minimize vehicle pitch motion in order to maintain level, horizontal flight along a survey track line for accurate synthetic aperture sonar imagery (Horner et al., 2009).

Before we proceed with the development of the control algorithm, it should be noted that quite often the UUV surge velocity is assumed to be constant, 0 *ut U* ( ) ≡ , to provide enough control authority in two other channels. This uniquely defines a throttle setting ( ) *<sup>T</sup>*δ *t* , and leaves only two control inputs, ( ) *<sup>s</sup>* δ *t* and ( ) *<sup>r</sup>* δ *t* , for altering the vehicle's trajectory. It also allows us to consider matrices **A** and **B** in (4) to be constant (time- and states-independent). If this assumption is not required, inverting kinematic and dynamic equations will differ slightly from the examples presented in the next section. However, the general ideas of the proposed approach remain unchanged.

#### **3. Real-time near-optimal guidance**

For the dynamic trajectory generator shown in Figs. 2 and 3, it is advocated to use the directmethod-based IDVD (Yakimenko, 2000). The primary rationale is that this approach features several important properties required for real-time implementations: i) the boundary conditions including high-order derivatives are satisfied *a priori*; ii) the resulting control commands are smooth and physically realizable, iii) the method is very robust and is not sensitive to small variations in input parameters, iv) any compound performance index can be used during optimization. Moreover, this specific method uses only a few variable parameters, thus ensuring that the iterative process during optimization converges very fast compared to other direct methods. The IDVD-based trajectory generator consists of several blocks. The goal of the first block, to be discussed next, is to produce a candidate trajectory, which satisfies the boundary conditions.

### **3.1 Generating a candidate trajectory**

70 Autonomous Underwater Vehicles

Finally, certain constraints should be obeyed by the state variables, controls and their derivatives. For example, in the case of a UUV these can include obvious constraints on the

where max *z xy* (,) describes a programmed operational depth limit. For vehicles programmed to operate at some nominal altitude above the sea floor, the max *z xy* (,) constraint can be converted into a minimum altitude min *h xy* (,) constraint as described in

where ℜ is the set of all known obstacle locations. The constraints are usually imposed not only on the controls themselves **δ δ** <sup>≤</sup> max but on their time derivatives as well **δ δ** <sup>≤</sup> max � � to account for actuator dynamics. Knowing the system's dynamics (4) (or simply complying with the autopilot specifications), these latter constraints can be elevated to the level of the

> ( )*t* ≤ and max ψ

The objective is to find the best trajectory and corresponding control inputs that minimize some performance index *J*. Typical performance index specifications include: i) minimizing time of the manoeuvre *<sup>f</sup>* <sup>0</sup> *t t* − , ii) minimizing the distance travelled to avoid the obstacle(s), and iii) minimizing control effort or energy consumption. In addition, the performance index may include some "exotic" constraints dictated by a sensor payload. For example, a UUV may require vehicle trajectories which point a fixed FLS at specified terrain features or minimize vehicle pitch motion in order to maintain level, horizontal flight along a survey

Before we proceed with the development of the control algorithm, it should be noted that quite often the UUV surge velocity is assumed to be constant, 0 *ut U* ( ) ≡ , to provide enough control authority in two other channels. This uniquely defines a throttle setting ( ) *<sup>T</sup>*

allows us to consider matrices **A** and **B** in (4) to be constant (time- and states-independent). If this assumption is not required, inverting kinematic and dynamic equations will differ slightly from the examples presented in the next section. However, the general ideas of the

For the dynamic trajectory generator shown in Figs. 2 and 3, it is advocated to use the directmethod-based IDVD (Yakimenko, 2000). The primary rationale is that this approach features

 ψ

max

 θ

track line for accurate synthetic aperture sonar imagery (Horner et al., 2009).

 *t* and ( ) *<sup>r</sup>* δ

δ

θ

0 0 **z z** ( ) *t* = , ( )*f f* **z z** *t* = (5)

min max *z zt z* ≤ ( ) ≤ (6)

[ ( ); ( ); ( )] 0 *xt yt zt* ∩ ℜ = (7)

� � ( )*t* ≤ (8)

*t* , for altering the vehicle's trajectory. It also

δ

*t* , and

Next, the admissible trajectories (1) should satisfy the initial and terminal conditions

UUV depth:

Section 6.2.

A 3D OA requirement can be formulated as

reference signals, for instance

leaves only two control inputs, ( ) *<sup>s</sup>*

proposed approach remain unchanged.

**3. Real-time near-optimal guidance** 

Again, consider the most general case of a UUV operating in 3D (as opposed to a USV). Suppose that each coordinate *<sup>i</sup> x* , *i* = 1,2,3 of the candidate UUV trajectory is represented as a polynomial of degree *M* of some abstract argument τ, the virtual arc

$$\chi\_i(\boldsymbol{\tau}) = \sum\_{k=0}^{M} a\_{ik} \boldsymbol{\tau}^k \; \tag{9}$$

(for simplicity of notation we assume <sup>1</sup> *x* () () τ ≡ *x* τ , <sup>2</sup> *x* () () τ ≡ *y* τ and <sup>3</sup> *x* () () τ ≡ *z* τ ). In general, analytic expressions for the trajectory coordinates can be constructed from any combination of basis functions to produce a rich variety of candidate trajectories. For example, a combination of monomials and trigonometric functions was utilized in (Yakimenko & Slegers, 2009).

As discussed in (Yakimenko, 2000; Horner & Yakimenko, 2007) the degree *M* is determined by the number of boundary conditions that must be satisfied. Specifically, it should be greater or equal to the number of preset boundary conditions but one. In general the desired trajectory includes constraints on the initial and final position, velocity and acceleration: *<sup>i</sup>*<sup>0</sup> *x* , *if x* , *<sup>i</sup>*<sup>0</sup> *x*′ , *if x*′ , *<sup>i</sup>*<sup>0</sup> *x*′′ , *if x*′′ . In this case the minimal order of polynomials (9) is 5, because all coefficients in (9) will be uniquely defined by these boundary conditions, leaving the "length" of the virtual arc *<sup>f</sup>* τ as the only varied parameter. For more flexibility in the candidate trajectory, additional varied parameters can be obtained by increasing the order of the polynomials (9). For instance, using seventh-order polynomials will introduce two more varied parameters for each coordinate expression. Rather than varying two coefficients in these extended polynomials directly, we will vary the initial and final jerk, *<sup>i</sup>*<sup>0</sup> *x*′′′ and *if x*′′′ , respectively. In this case, coefficients *ik a* in (9) can be determined by solving the obvious system of linear algebraic equations equating polynomials (9) to *<sup>i</sup>*<sup>0</sup> *x* , *if x* , *<sup>i</sup>*<sup>0</sup> *x*′ , *if x*′ , *<sup>i</sup>*<sup>0</sup> *x*′′ , *if x*′′ , *<sup>i</sup>*<sup>0</sup> *x*′′′ and *if x*′′′ at two endpoints (τ = 0 and *<sup>f</sup>* τ =τ) (Yakimenko, 2000, 2008).

By construction, the boundary conditions (5) will be satisfied unconditionally for any value of the final arc *<sup>f</sup>* τ . However, varying *<sup>f</sup>* τ will alter the shape of the candidate trajectory. Figure 4 demonstrates a simple example whereby a UUV operating 2m above the sea floor at 1.5m/s must perform a pop-up manoeuvre to avoid some obstacle. Even with a single varied parameter, changing the value of *<sup>f</sup>* τ allows the UUV to avoid obstacles of different heights. Similar trajectories could be produced solely in the horizontal plane or in all three dimensions. It should be pointed out that even at this stage infeasible candidate trajectories

Real-Time Optimal Guidance and Obstacle Avoidance for UMVs 73

Obviously, we cannot allow this to happen because we want to vary the speed profile

τ

( ) *<sup>d</sup> dt* τ

<sup>222</sup> *V xyz* () () () () ()

The capability to satisfy higher-order derivatives at the trajectory endpoints, specifically at the initial point, allows continuous regeneration of the trajectory to accommodate sudden changes like newly discovered obstacles. As an example, Fig.7 demonstrates a scenario whereby a UUV executing an OA manoeuvre discovers a second obstacle and must generate a new trajectory beginning with the current vehicle states and control values (up to the second-order derivatives of the states). The suggested approach enables this type of

The second key block inside the dynamic trajectory generator in Figs. 2 and 3 accepts the candidate trajectory as an input and computes the components of the state vector and control signals required to follow it. In this way we can ensure that each candidate trajectory

> () ()() *d d d dt* ζ τ

τ

τ

τ

ττ

τ

 ζ τ λτ

domain

<sup>0</sup> ( ) () () () () () *u b x U y Rv z w*

<sup>⎡</sup> ′ ⎤ ⎡⎤ <sup>⎢</sup> ⎥ ⎢⎥ ′ <sup>=</sup> <sup>⎢</sup> ⎥ ⎢⎥ <sup>⎢</sup> ′ ⎥ ⎢⎥ <sup>⎣</sup> ⎦ ⎣⎦

ζ,

> τ

> > τ

= = ′ (13)

θ

(14)

*t* ≈ and cos ( ) 1

θ

*t* ≈ , so

 τ  τ

= ++

 τ

we can achieve any desired speed profile.

λ τ

τ

Fig. 7. Example of dynamic trajectory reconfiguration

does not violate any constraints (including those of (8)). First, using the following relation for any parameter

we convert kinematic equations (2) into the

that the rotation matrix (3) becomes

ζ τ

> λ

Next, we assume the pitch angle to be small enough to let sin ( ) 0

λτ

continuous trajectory generation and ensures smooth, non-shock transitions.

independently. With the abstract argument

such that

speed factor

and by varying

**3.2 Inverse dynamics** 

λ

Now instead of (10) we have

λ( ) τ

22 2 2 22 *Vt ut vt wt xt* () () () () () () () = ++ = ++ *y t zt* (10)

this becomes possible via introduction of a

= . (11)

′′′ (12)

will be ruled out. (In Fig.4 the trajectory requiring the UUV to jump out of the water is infeasible because it violates the constraint (6).)

Fig. 4. Varying the candidate trajectory while changing the value of τ*f*

With six free parameters, which in our case are components of the initial and final jerk ( *<sup>i</sup>*<sup>0</sup> *x*′′′ and *if x*′′′ , *i* = 1,2,3 ) the trajectory generator can change the overall shape of the trajectory even further. To this end, Fig.5 illustrates candidate trajectories for a UUV avoiding a 10m obstacle located between its initial and final points. These trajectories were generated by varying just two components of the jerk*,* <sup>30</sup> *x*′′′ and <sup>3</sup> *<sup>f</sup> x*′′′ , and minimizing *<sup>f</sup>* τ . This additional flexibility can produce trajectories which satisfy operational constraints (6), as well as OA constraints (7).

Fig. 5. Candidate trajectories obtained by varying the terminal jerks

The selection of a specific trajectory will be based upon whether the trajectory is feasible (satisfies constraints (8)) and if so, whether it assures the minimum value of the performance index calculated using the values of the vehicle states (and controls) along that trajectory. As an example, Fig.6 presents collision-free solutions for two different locations of a 10m-tall obstacle when five varied parameters, <sup>10</sup> *x*′′′ , <sup>1</sup> *<sup>f</sup> x*′′′ , <sup>30</sup> *x*′′′ , <sup>3</sup> *<sup>f</sup> x*′′′ and *<sup>f</sup>* τ , are optimized to assure feasible minimum-path-length trajectories

Fig. 6. Examples of minimum-path-length trajectories

Now, let us address the reason for choosing some abstract parameter τ as an argument for the reference functions (9) rather than time or path length, which are commonly used. Assume for a moment that τ ≡ *t* . In this case, once we determine the trajectory we unambiguously define a speed profile along this trajectory as well, since

$$V(t) = \sqrt{\mu(t)^2 + v(t)^2 + w(t)^2} = \sqrt{\dot{x}(t)^2 + \dot{y}(t)^2 + \dot{z}(t)^2} \tag{10}$$

Obviously, we cannot allow this to happen because we want to vary the speed profile independently. With the abstract argument τ this becomes possible via introduction of a speed factor λsuch that

$$
\mathcal{A}(\tau) = \frac{d\tau}{dt} \,. \tag{11}
$$

Now instead of (10) we have

72 Autonomous Underwater Vehicles

will be ruled out. (In Fig.4 the trajectory requiring the UUV to jump out of the water is

With six free parameters, which in our case are components of the initial and final jerk ( *<sup>i</sup>*<sup>0</sup> *x*′′′ and *if x*′′′ , *i* = 1,2,3 ) the trajectory generator can change the overall shape of the trajectory even further. To this end, Fig.5 illustrates candidate trajectories for a UUV avoiding a 10m obstacle located between its initial and final points. These trajectories were generated by

flexibility can produce trajectories which satisfy operational constraints (6), as well as OA

The selection of a specific trajectory will be based upon whether the trajectory is feasible (satisfies constraints (8)) and if so, whether it assures the minimum value of the performance index calculated using the values of the vehicle states (and controls) along that trajectory. As an example, Fig.6 presents collision-free solutions for two different locations of a 10m-tall

the reference functions (9) rather than time or path length, which are commonly used.

τ*f*

τ

≡ *t* . In this case, once we determine the trajectory we

τ

τ

. This additional

, are optimized to assure

as an argument for

infeasible because it violates the constraint (6).)

constraints (7).

Fig. 4. Varying the candidate trajectory while changing the value of

varying just two components of the jerk*,* <sup>30</sup> *x*′′′ and <sup>3</sup> *<sup>f</sup> x*′′′ , and minimizing *<sup>f</sup>*

Fig. 5. Candidate trajectories obtained by varying the terminal jerks

obstacle when five varied parameters, <sup>10</sup> *x*′′′ , <sup>1</sup> *<sup>f</sup> x*′′′ , <sup>30</sup> *x*′′′ , <sup>3</sup> *<sup>f</sup> x*′′′ and *<sup>f</sup>*

Now, let us address the reason for choosing some abstract parameter

unambiguously define a speed profile along this trajectory as well, since

τ

feasible minimum-path-length trajectories

Assume for a moment that

Fig. 6. Examples of minimum-path-length trajectories

$$V(\tau) = \lambda(\tau)\sqrt{x'(\tau)^2 + y'(\tau)^2 + z'(\tau)^2} \tag{12}$$

and by varying λ( ) τwe can achieve any desired speed profile.

The capability to satisfy higher-order derivatives at the trajectory endpoints, specifically at the initial point, allows continuous regeneration of the trajectory to accommodate sudden changes like newly discovered obstacles. As an example, Fig.7 demonstrates a scenario whereby a UUV executing an OA manoeuvre discovers a second obstacle and must generate a new trajectory beginning with the current vehicle states and control values (up to the second-order derivatives of the states). The suggested approach enables this type of continuous trajectory generation and ensures smooth, non-shock transitions.

Fig. 7. Example of dynamic trajectory reconfiguration

#### **3.2 Inverse dynamics**

The second key block inside the dynamic trajectory generator in Figs. 2 and 3 accepts the candidate trajectory as an input and computes the components of the state vector and control signals required to follow it. In this way we can ensure that each candidate trajectory does not violate any constraints (including those of (8)).

First, using the following relation for any parameter ζ,

$$
\dot{\zeta}(\tau) = \frac{d\zeta}{d\tau} \frac{d\tau}{dt} = \zeta'(\tau)\lambda(\tau) \tag{13}
$$

we convert kinematic equations (2) into the τdomain

$$
\mathcal{A}(\boldsymbol{\pi}) \begin{bmatrix} \boldsymbol{\pi}'(\boldsymbol{\pi}) \\ \boldsymbol{y}'(\boldsymbol{\pi}) \\ \boldsymbol{z}'(\boldsymbol{\pi}) \end{bmatrix} = {}^{\boldsymbol{u}}\_{b} \boldsymbol{R} \begin{bmatrix} \boldsymbol{U}\_{0} \\ \boldsymbol{v}(\boldsymbol{\pi}) \\ \boldsymbol{w}(\boldsymbol{\pi}) \end{bmatrix} \tag{14}
$$

Next, we assume the pitch angle to be small enough to let sin ( ) 0 θ *t* ≈ and cos ( ) 1 θ *t* ≈ , so that the rotation matrix (3) becomes

Real-Time Optimal Guidance and Obstacle Avoidance for UMVs 75

We proceed with computing the remaining states along the reference trajectory over a fixed

In order to determine coefficients for polynomials (9) we will have to guess on the values of

known or desired boundary conditions *<sup>i</sup>*<sup>0</sup> *x* , *<sup>i</sup>*<sup>0</sup> *x*′ , *<sup>i</sup>*<sup>0</sup> *x*′′ , *if x* , *if x*′ , and *if x*′′ . The boundary conditions on coordinates *<sup>i</sup>*<sup>0</sup> *x* and *if x* come directly from (5). According to (14), the given boundary conditions on surge, sway, and heave velocities define the first-order time

> 0; 0 1 0; 0; 0; 0; 0; 0;

<sup>⎢</sup> ′ ⎢⎣ ⎦ ⎣⎦

*x U y Rv z w* λ− <sup>⎡</sup> ′ ⎤ ⎡⎤ <sup>⎢</sup> ⎥ ⎢⎥ <sup>⎢</sup> ′ <sup>=</sup>

*u f f b f f f f*

<sup>1</sup> ( 1)

τ

, *<sup>i</sup>*<sup>0</sup> *x*′′′ , *<sup>i</sup>*<sup>0</sup> *x*′′′ , *if x*′′′ , and *if x*′′′ . These guesses will be used along with the

 and <sup>1</sup> 0; 0; 0

*f*

ψ

. This follows directly from equations (11) and (12) that 1 1

1 *j j* 1

01 1

*j j*

− −

− −−

( )( )( ) *jj j j jj*

− +− +−

*Uv w*

+ +

2 22 1 11

*<sup>f</sup> <sup>N</sup>* <sup>−</sup> Δ= − (20)

τ

= 0 ) (21)

(22)

*u*

λ

λ τ 0; *ff f U s*<sup>0</sup> − − = ,

λ

(25)

0

<sup>−</sup> =Ψ − (23)

tan *<sup>f</sup>*

<sup>−</sup> =Δ Δ <sup>−</sup> (24)

*v U* *bR <sup>f</sup>* in (22) as

, for example,

, the larger

with the

set of *N* points (for instance, *N*=100) spaced evenly along the virtual arc [0; ]*<sup>f</sup>*

τ τ

 τ= <sup>−</sup> + Δ , *j N* = 2,..., , ( <sup>1</sup>

*f*

They also define the initial and final pitch and yaw angles used to compute 0;

*w U v*

0 0;

+

*f*

Finally, initial values for the second-order derivatives are provided by the UUV motion sensors (see Figs. 1-3) (after conversion to the *τ* domain), while final values for the secondorder derivatives are usually set to zero for a smooth arrival at the final point. Having an analytical representation of the candidate trajectory (9) defines the values of *ij x* , and *ij x*′ ,

> τ*t*

1 0;

In equation (22) we may use any value for the initial and final speed factor

*<sup>f</sup>* = . This value simply scales the virtual domain; the higher the values for

λ

<sup>1</sup> 22 2

*xx yy zz <sup>t</sup>*

tan *<sup>f</sup>*

0; 0 2 2

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

*j j* 1 τ τ

**3.3 Discretization** 

the varied parameters *<sup>f</sup>*

derivatives of the coordinates as

τ

*f*

 γ

θ

τ

where *<sup>f</sup> s* is the physical path length.

Now, for each node *j* =1,...,*N* we compute

*j*

−

Δ =

interval

so that

0; 1 λ

where

the values for *<sup>f</sup>*

*i* = 1,2,3 , *j N* = 1,..., .

$$\begin{aligned} \, \_b^\* R(\tau) = \begin{bmatrix} \cos \psi(\tau) & -\sin \psi(\tau) & 0 \\ \sin \psi(\tau) & \cos \psi(\tau) & 0 \\ 0 & 0 & 1 \end{bmatrix} \end{aligned} \tag{15}$$

While this step is not required, it simplifies the expressions in the following development. Inverting (14) via the rotation matrix (15) yields

$$
\begin{bmatrix} \mathcal{U}\_0 \\ v \\ w \end{bmatrix} = \mathcal{A} \begin{bmatrix} \cos \psi & \sin \psi & 0 \\ -\sin \psi & \cos \psi & 0 \\ 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} x' \\ y' \\ z' \end{bmatrix} \tag{16}
$$

Hereafter each variable's explicit dependence on τ will be omitted from the notation. Now the three equations of system (16) must be resolved with respect to three unknown parameters, *v*, *w* and *ψ*. While the last one readily yields

$$w = \mathcal{X}z'\tag{17}$$

the first two require more rigorous analysis.

Consider Fig.8. Geometrically, a scalar product of two vectors on the right-hand-side of the first equation in (16) represents the length of the longest side of the shaded rectangle. Similarly, the second equation expresses the length of the shortest side of this rectangle. From here it follows that the square of the length of the diagonal vector can be expressed in two ways: 22 22 2 2 <sup>0</sup> *vU x* λ λ*y* − − + =+ ′ ′ . This yields

$$v = \sqrt{\lambda^2 \left(\mathbf{x'}^2 + \mathbf{y'^2}\right) - \mathbf{U}\_0^2} \tag{18}$$

From the same figure it follows that

$$\text{V}\,\varphi = \text{"\'P} - \tan^{-1}\frac{\nu\text{\mathscr{X}}^{-1}}{U\_o\text{\mathscr{X}}^{-1}} = \text{"\'P} - \tan^{-1}\frac{\nu}{U\_o}\text{"\'Q} = \tan^{-1}\frac{\text{y}^{\prime}}{\text{x}^{\prime}}\tag{19}$$

Fig. 8. Kinematics of horizontal plane parameters

Now, using these inverted kinematic equations, we can check whether each candidate trajectory obeys the constraints imposed on it (constraints (8)).

#### **3.3 Discretization**

We proceed with computing the remaining states along the reference trajectory over a fixed set of *N* points (for instance, *N*=100) spaced evenly along the virtual arc [0; ]*<sup>f</sup>* τ with the interval

$$
\Delta \tau = \tau\_f (\text{N} - \text{1})^{-1} \tag{20}
$$

so that

74 Autonomous Underwater Vehicles

*R ψ ψ* τ

While this step is not required, it simplifies the expressions in the following development.

<sup>0</sup> cos sin 0

λ

*U ψ ψ x v ψ ψ y w z*

⎡ ⎤ ⎡ ⎤⎡ ⎤′

<sup>⎢</sup> ⎥ ⎢ ⎥⎢ ⎥ = − ′ <sup>⎢</sup> ⎥ ⎢ ⎥⎢ ⎥ <sup>⎢</sup> ⎥ ⎢ ⎥⎢ ⎥′ <sup>⎣</sup> ⎦ ⎣ ⎦⎣ ⎦

Now the three equations of system (16) must be resolved with respect to three unknown

*w z* = λ

Consider Fig.8. Geometrically, a scalar product of two vectors on the right-hand-side of the first equation in (16) represents the length of the longest side of the shaded rectangle. Similarly, the second equation expresses the length of the shortest side of this rectangle. From here it follows that the square of the length of the diagonal vector can be expressed in

> 22 2 2 <sup>0</sup> *v x* = +−

*U U* , <sup>1</sup> tan *<sup>y</sup>*

0 0

*ψ* Ψ

Now, using these inverted kinematic equations, we can check whether each candidate

<sup>1</sup> *U*0λ−

λ

1 1 1 1

− − − <sup>−</sup> =Ψ− =Ψ− *v v*

λ

tan tan λ

*u b*

Inverting (14) via the rotation matrix (15) yields

Hereafter each variable's explicit dependence on

the first two require more rigorous analysis.

 λ

> ψ

Fig. 8. Kinematics of horizontal plane parameters

two ways: 22 22 2 2 <sup>0</sup> *vU x*

From the same figure it follows that

λ

parameters, *v*, *w* and *ψ*. While the last one readily yields

*y* − − + =+ ′ ′ . This yields

<sup>1</sup> *v*λ−

trajectory obeys the constraints imposed on it (constraints (8)).

τ

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

ττ

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

sin cos 0 0 01

τ

*ψ ψ*

0 01

 τ

(15)

(16)

will be omitted from the notation.

′ (17)

( ) ′ ′ *y U* (18)

*x*

(19)

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

cos sin

*x*

*x y* ⎡ ′⎤ ⎢ ⎥ ′ ⎣ ⎦

<sup>1</sup> =tan *<sup>y</sup>*

<sup>−</sup> ′ <sup>Ψ</sup> ′

*ψ ψ* <sup>⎡</sup> <sup>⎤</sup> <sup>⎢</sup> <sup>⎥</sup> <sup>⎣</sup> <sup>⎦</sup>

$$
\pi\_j = \pi\_{j-1} + \Delta \pi\_{\prime \prime} \quad j = \mathcal{D}, \ldots, N \; \; \; \; \; \; \; \; \; \; \; \; \; \; \; \tag{21}
$$

In order to determine coefficients for polynomials (9) we will have to guess on the values of the varied parameters *<sup>f</sup>* τ , *<sup>i</sup>*<sup>0</sup> *x*′′′ , *<sup>i</sup>*<sup>0</sup> *x*′′′ , *if x*′′′ , and *if x*′′′ . These guesses will be used along with the known or desired boundary conditions *<sup>i</sup>*<sup>0</sup> *x* , *<sup>i</sup>*<sup>0</sup> *x*′ , *<sup>i</sup>*<sup>0</sup> *x*′′ , *if x* , *if x*′ , and *if x*′′ . The boundary conditions on coordinates *<sup>i</sup>*<sup>0</sup> *x* and *if x* come directly from (5). According to (14), the given boundary conditions on surge, sway, and heave velocities define the first-order time derivatives of the coordinates as

$$
\begin{bmatrix} \boldsymbol{x}'\_{0;f} \\ \boldsymbol{y}'\_{0;f} \\ \boldsymbol{z}'\_{0;f} \end{bmatrix} = \boldsymbol{\mathcal{X}}\_{0;f}^{-1} \, {}^{\mu} \mathbf{R}\_{0;f} \begin{bmatrix} \boldsymbol{U}\_{0} \\ \boldsymbol{w}\_{0;f} \\ \boldsymbol{w}\_{0;f} \end{bmatrix} \tag{22}
$$

They also define the initial and final pitch and yaw angles used to compute 0; *u bR <sup>f</sup>* in (22) as

$$\theta\_{0;f} = \gamma\_0 + \tan^{-1} \frac{-w\_{0;f}}{\sqrt{\mathcal{U}\_0^2 + v\_{0;f}^2}} \text{ and } \psi\_{0;f} = \Psi\_0 - \tan^{-1} \frac{v\_{0;f}}{\mathcal{U}\_0} \tag{23}$$

In equation (22) we may use any value for the initial and final speed factor λ , for example, 0; 1 λ *<sup>f</sup>* = . This value simply scales the virtual domain; the higher the values for λ , the larger the values for *<sup>f</sup>* τ . This follows directly from equations (11) and (12) that 1 1 λ τ 0; *ff f U s*<sup>0</sup> − − = , where *<sup>f</sup> s* is the physical path length.

Finally, initial values for the second-order derivatives are provided by the UUV motion sensors (see Figs. 1-3) (after conversion to the *τ* domain), while final values for the secondorder derivatives are usually set to zero for a smooth arrival at the final point. Having an analytical representation of the candidate trajectory (9) defines the values of *ij x* , and *ij x*′ , *i* = 1,2,3 , *j N* = 1,..., .

Now, for each node *j* =1,...,*N* we compute

$$\mathcal{A}\_{j} = \Delta \pi \Delta t\_{j-1}^{-1} \tag{24}$$

where

$$\Delta t\_{j-1} = \frac{\sqrt{(x\_j - x\_{j-1})^2 + (y\_j - y\_{j-1})^2 + (z\_j - z\_{j-1})^2}}{\sqrt{\mathcal{U}\_0^2 + v\_{j-1}^2 + w\_{j-1}^2}}\tag{25}$$

Real-Time Optimal Guidance and Obstacle Avoidance for UMVs 77

generates a new trajectory to steer left and pass safely behind the object's stern. The

For the case of a UUV manoeuvring in the vertical plane, the 3D algorithm can be reduced to the 2D case in a manner similar to the horizontal case. Specifically, using five varied

and then use the same general equations developed in Section 3, assuming *y* ≡ 0 , *y*′ ≡ 0 ,

Alternatively, we can use a single reference polynomial to approximate just 3 *x* and then integrate the third equation of (4) to get the heave velocity *w* . That allows computation of

Another way of dealing with vertical plane manoeuvres is to invert the dynamic equations (4) (Horner & Yakimenko, 2007). After developing the reference functions for two

θ

1 1 1 10 1 1 1

*j j j j j jj j jj j j s j jj j*

′ = + = +Δ ′

− −− − − − −

*xw U xx x w wq ww w*

1 1 33 1 35 1 32 ; 1 1 1

− −− − − − −

cos , , *j jj j j j j*

 θ

Finally, we can compute the dive plane deflection required to follow the trajectory using the

⎛ ⎞ ′ ′ + −− <sup>=</sup> ⎜ ⎟ =≈ =≈ <sup>+</sup> Δ Δ ⎝ ⎠

1 0 1 1

<sup>−</sup> <sup>−</sup> <sup>−</sup>

*ux wz q q q q w U t t* θ θ

*j j j*

δ

In this case, the corresponding time period *<sup>j</sup>* <sup>1</sup> *t* Δ <sup>−</sup> is computed similarly to (28):

and the heave velocity is calculated using the third equation of system (4) as

( sin cos ) A AB

λθθ

2 2

( )

*j j j j j j*

The next step involves computing the pitch angle, pitch rate and pitch acceleration as

1

1 10 1 1 cos sin *j j j j*

′ <sup>=</sup> + + = +Δ ′ (30)

0 1 1

*zz zz*

 θ

*jj j j*

−− − − − − Δ =− = <sup>≈</sup> <sup>+</sup> (29)

*wu w*

1 11 ( ) *j jj j t zz w*<sup>−</sup> Δ =− <sup>−</sup> − − (28)

1 1

− −

δτ

 θ

− −

control input is computed subject to five variable

 τ

(31)

, <sup>10</sup> *x*′′′ , <sup>30</sup> *x*′′′ , <sup>1</sup> *<sup>f</sup> x*′′′ and <sup>3</sup> *<sup>f</sup> x*′′′ , we can develop reference trajectories for *x*1 and *x*3,

complete trajectory is shown as a solid line.

Fig. 9. Moving obstacle avoidance in a horizontal plane

**4.2 Vertical plane guidance** 

τ

the time period *<sup>j</sup>* <sup>1</sup> *t* Δ <sup>−</sup> using

τ

coordinates, <sup>1</sup> *x* and <sup>3</sup> *x* , the stern plane *<sup>s</sup>*

, <sup>10</sup> *x*′′′ , <sup>30</sup> *x*′′′ , <sup>1</sup> *<sup>f</sup> x*′′′ , and <sup>3</sup> *<sup>f</sup> x*′′′ .

1 1

*j jj*

− −

*t tt*

1

−

1

−

λ

θλ

5th equation of system (4) as

instead of (25).

parameters: *<sup>f</sup>*

parameters, *<sup>f</sup>*

and 0 Ψ ≡ .

and then use (17)-(19) to compute *w*, *v*, *ψ* and Ψ at each timestamp. The vertical plane parameters, flight path angle *γ* and pitch angle *θ*, can be computed using the following relations:

$$\gamma\_j = \tan^{-1} \frac{-z\_j'}{\sqrt{x\_j'^2 + y\_j'^2}}, \quad \theta\_j \approx \gamma\_j + \tan^{-1} \frac{-w\_j}{\sqrt{\mathcal{U}\_0^2 + v\_j^2}} \tag{26}$$

In order to check the yaw rate constraints (8) we must first numerically differentiate the expression for Ψ in (19).

## **3.4 Optimization**

When all parameters (states and controls) are computed in each of *N* points, we can compute the performance index *J* and the penalty function. For example, we can combine constraints (6) and (8) into the joint penalty

$$\Delta = \left[ k^{z\_{\min}}, k^{z\_{\max}}, k^{\theta}, k^{\nu} \right] \left| \begin{array}{c} \min\_{j} \left( 0; z\_{j} - z\_{\min} \right)^{2} \\ \max\_{j} \left( 0; z\_{j} - z\_{\max} \right)^{2} \\ \max\_{j} \left( 0; \left| \theta\_{j} \right| - \theta\_{\max} \right)^{2} \\ \max\_{j} \left( 0; \left| \boldsymbol{\nu}\_{j} \right| - \boldsymbol{\nu}\_{\max} \right)^{2} \end{array} \tag{27}$$

with min *<sup>z</sup> k* , max *<sup>z</sup> k* , *k*θ and *k* ψ being scaling (weighting) coefficients. Now the problem can be solved using numerical methods such as the built-in *fmincon* function in the Mathworks' MATLAB development environment. Alternatively, by combining the performance index *J* with the joint penalty Δ we may exploit MATLAB's non-gradient *fminsearch* function. For real-time applications, however, the authors prefer to use a more robust optimization routine based on the gradient-free Hooke-Jeeves pattern search algorithm (Yakimenko, 2011).

#### **4. Planar cases**

This section presents two simplified cases for a vehicle manoeuvring exclusively in either the horizontal or vertical plane.

#### **4.1 Horizontal plane guidance**

For the case of a UUV manoeuvring in the horizontal plane or a USV, the computational procedure is simplified. The trajectory is represented by only two reference polynomials for coordinates *x*1 and *x*2. Hence, we end up having only five varied parameters, which are: *<sup>f</sup>* τ , <sup>10</sup> *x*′′′ , <sup>20</sup> *x*′′′ , <sup>1</sup> *<sup>f</sup> x*′′′ and <sup>2</sup> *<sup>f</sup> x*′′′ . The remaining kinematic formulas are identical to those presented above with *z* ≡ 0 , *z*′ ≡ 0 and γ ≡ 0 . Figure 9 shows an example of a planar scenario in which a USV has to compute a new trajectory twice. First, after detecting an obstacle blocking its original path, a new trajectory is generated to steer right and pass safely in front of the object (dotted line). Second, while executing the first avoidance manoeuvre the USV detects that the object has moved south into its path. It therefore generates a new trajectory to steer left and pass safely behind the object's stern. The complete trajectory is shown as a solid line.

#### **4.2 Vertical plane guidance**

76 Autonomous Underwater Vehicles

and then use (17)-(19) to compute *w*, *v*, *ψ* and Ψ at each timestamp. The vertical plane parameters, flight path angle *γ* and pitch angle *θ*, can be computed using the following

> θ γ

In order to check the yaw rate constraints (8) we must first numerically differentiate the

When all parameters (states and controls) are computed in each of *N* points, we can compute the performance index *J* and the penalty function. For example, we can combine

, ,, max(0; )

*<sup>j</sup> <sup>j</sup> <sup>z</sup> <sup>z</sup>*

⎣ ⎦ <sup>−</sup>

solved using numerical methods such as the built-in *fmincon* function in the Mathworks' MATLAB development environment. Alternatively, by combining the performance index *J* with the joint penalty Δ we may exploit MATLAB's non-gradient *fminsearch* function. For real-time applications, however, the authors prefer to use a more robust optimization routine

This section presents two simplified cases for a vehicle manoeuvring exclusively in either

For the case of a UUV manoeuvring in the horizontal plane or a USV, the computational procedure is simplified. The trajectory is represented by only two reference polynomials for coordinates *x*1 and *x*2. Hence, we end up having only five varied parameters, which

scenario in which a USV has to compute a new trajectory twice. First, after detecting an obstacle blocking its original path, a new trajectory is generated to steer right and pass safely in front of the object (dotted line). Second, while executing the first avoidance manoeuvre the USV detects that the object has moved south into its path. It therefore

γ

, <sup>10</sup> *x*′′′ , <sup>20</sup> *x*′′′ , <sup>1</sup> *<sup>f</sup> x*′′′ and <sup>2</sup> *<sup>f</sup> x*′′′ . The remaining kinematic formulas are identical to those

θ ψ

based on the gradient-free Hooke-Jeeves pattern search algorithm (Yakimenko, 2011).

, <sup>1</sup>

*j j*

2 2 0

+

2 min

2 max

2 max

2 max

≡ 0 . Figure 9 shows an example of a planar

*w U v*

*j*

(26)

(27)

tan *<sup>j</sup>*

<sup>−</sup> <sup>−</sup> ≈ +

min(0; )

⎡ ⎤ <sup>−</sup> ⎢ ⎥

*z z*

*z z*

−

*<sup>j</sup> <sup>j</sup>*

*<sup>j</sup> <sup>j</sup>*

*<sup>j</sup> <sup>j</sup>*

max(0; )

θ θ

max(0; )

<sup>−</sup> ⎣ ⎦

being scaling (weighting) coefficients. Now the problem can be

ψ ψ

1

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

*j*

γ

constraints (6) and (8) into the joint penalty

θ and *k* ψ

tan *<sup>j</sup>*

min max

Δ = ⎡ ⎤

*k k kk*

2 2

*j j*

′ + ′

*z x y*

relations:

expression for Ψ in (19).

**3.4 Optimization** 

with min *<sup>z</sup> k* , max *<sup>z</sup> k* , *k*

**4. Planar cases** 

are: *<sup>f</sup>* τ

the horizontal or vertical plane.

**4.1 Horizontal plane guidance** 

presented above with *z* ≡ 0 , *z*′ ≡ 0 and

For the case of a UUV manoeuvring in the vertical plane, the 3D algorithm can be reduced to the 2D case in a manner similar to the horizontal case. Specifically, using five varied parameters, *<sup>f</sup>* τ , <sup>10</sup> *x*′′′ , <sup>30</sup> *x*′′′ , <sup>1</sup> *<sup>f</sup> x*′′′ and <sup>3</sup> *<sup>f</sup> x*′′′ , we can develop reference trajectories for *x*1 and *x*3, and then use the same general equations developed in Section 3, assuming *y* ≡ 0 , *y*′ ≡ 0 , and 0 Ψ ≡ .

Fig. 9. Moving obstacle avoidance in a horizontal plane

Alternatively, we can use a single reference polynomial to approximate just 3 *x* and then integrate the third equation of (4) to get the heave velocity *w* . That allows computation of the time period *<sup>j</sup>* <sup>1</sup> *t* Δ <sup>−</sup> using

$$
\Delta t\_{j-1} = (z\_j - z\_{j-1})w\_{j-1}^{-1} \tag{28}
$$

instead of (25).

Another way of dealing with vertical plane manoeuvres is to invert the dynamic equations (4) (Horner & Yakimenko, 2007). After developing the reference functions for two coordinates, <sup>1</sup> *x* and <sup>3</sup> *x* , the stern plane *<sup>s</sup>* δ control input is computed subject to five variable parameters: *<sup>f</sup>* τ, <sup>10</sup> *x*′′′ , <sup>30</sup> *x*′′′ , <sup>1</sup> *<sup>f</sup> x*′′′ , and <sup>3</sup> *<sup>f</sup> x*′′′ .

In this case, the corresponding time period *<sup>j</sup>* <sup>1</sup> *t* Δ <sup>−</sup> is computed similarly to (28):

$$\Delta t\_{j-1} = t\_j - t\_{j-1} = \frac{z\_j - z\_{j-1}}{w\_{j-1}\cos\theta\_{j-1} + u\_0 \sin\theta\_{j-1}} \approx \frac{z\_j - z\_{j-1}}{w\_{j-1}} \tag{29}$$

and the heave velocity is calculated using the third equation of system (4) as

$$\begin{aligned} \mathbf{x}'\_{j-1} &= \boldsymbol{\lambda}^{-1}\_{j-1} (\boldsymbol{w}\_{j-1} \sin \theta\_{j-1} + \boldsymbol{U}\_0 \cos \theta\_{j-1}) & \mathbf{x}\_j &= \mathbf{x}\_{j-1} + \boldsymbol{\Delta} \tau \mathbf{x}'\_{j-1} \\ \mathbf{w}'\_{j-1} &= \boldsymbol{\lambda}^{-1}\_{j-1} \left( \mathbf{A}\_{33} \mathbf{w}\_{j-1} + \mathbf{A}\_{33} \mathbf{q}\_{j-1} + \mathbf{B}\_{32} \boldsymbol{\delta}\_{s;j-1} \right) & \mathbf{w}\_j &= \mathbf{w}\_{j-1} + \boldsymbol{\Delta} \tau \mathbf{w}'\_{j-1} \end{aligned} \tag{30}$$

The next step involves computing the pitch angle, pitch rate and pitch acceleration as

$$\theta\_j = \cos^{-1}\left(\mathcal{A}\_j \frac{u\_0 x\_j' + w\_j x\_j'}{w\_j^2 + \mathcal{U}\_0^2}\right), \ q\_j = \dot{\theta}\_j \approx \frac{\theta\_j - \theta\_{j-1}}{\Delta t\_{j-1}}, \ \dot{q}\_j = \ddot{\theta}\_j \approx \frac{q\_j - q\_{j-1}}{\Delta t\_{j-1}} \tag{31}$$

Finally, we can compute the dive plane deflection required to follow the trajectory using the 5th equation of system (4) as

Real-Time Optimal Guidance and Obstacle Avoidance for UMVs 79

To support ongoing CAVR research into sonar-based OA, terrain-relative navigation, and multi-vehicle operations in cluttered environments, each NPS REMUS vehicle has been modified to incorporate a FLS, multi-beam bathymetric sonar, acoustic communications modem, navigation-grade inertial measurement system, and fore/aft horizontal/vertical cross-body thrusters for hovering or precise manoeuvring. (Figure 10b provides a close up of the NPS REMUS FLS arrays with nose cap removed.) To maximize the REMUS system's utility as a research platform, Hydroid developed the RECON communications interface so that sensor and computer payloads can interact with the REMUS autopilot. Using this interface, NPS payloads receive vehicle sensor data and generate autopilot commands based

The SeaFox USV was designed and manufactured by Northwind Marine (Seattle, WA) as a remote-controlled platform for intelligence, surveillance, reconnaissance, anti-terrorist force protection, and maritime interdiction operations (Northwind Marine, 2011). SeaFox is a 4.88m long, aluminium, rigid-hulled inflatable boat with a 1.75m beam; 0.25m draft; folddown communications mast; and fully-enclosed electronics and engine compartments. SeaFox's water jet propulsion system is powered by a JP5-fueled, 185-HP V-6 Mercury Racing engine, and can deliver a top speed of 74km/h. Standard sensing systems include three daylight and three low light navigation cameras for remote operation, as well as twin daylight and infrared gyro-stabilized camera turrets for video surveillance. All video is

The NPS SeaFox was modified to enable fully-autonomous operations by integrating a payload computer with the primary autopilot (Fig.11). Meanwhile, the original remote control link was retained to provide an emergency stop function. NPS algorithms running on the payload computer generate rudder and throttle commands that are sent directly to the SeaFox autopilot. Recent navigational upgrades include a satellite compass that uses differential Global Positioning System (GPS) navigation service for accurate heading information, a tactical-grade inertial measurement unit for precise attitude estimation, and an optional ADCP/DVL for water velocity measurements. To support ongoing CAVR research into autonomous riverine navigation, the NPS SeaFox was further upgraded to deploy a retractable, pole-mounted FLS system for underwater obstacle detection and avoidance (Gadre et al., 2009). Figure 12 shows the SeaFox USV operating on a river with its

The NPS REMUS and SeaFox vehicles rely on FLS to detect and localize obstacles in their environment. Both platforms utilize commercial blazed array sonar systems manufactured by BlueView Technologies (BlueView Technologies, 2011). These sonar systems comprise one or more pairs of arrays grouped into sonar "heads." Each sonar head generates a 2D cross-sectional image of the water column in polar coordinates, typically plotted as the image plane's field of view angle vs. range. Due to the sonar arrays' beam width, the resulting FLS imagery has a 12-degree out-of-plane ambiguity. The REMUS FLS system consists of two fixed sonar heads, which provide a 90-degree horizontal field of view (FOV) and a 45-degree vertical FOV. Similarly, the SeaFox FLS system is comprised of twin sonar heads mounted on port and starboard pan/tilt actuators, providing each side with a 45 degree FOV image at an adjustable mounting orientation that can be swept through the

on NPS sonar processing, trajectory generation, and path-following algorithms.

accessible over a wireless network via two onboard video servers.

sonar system deployed below the waterline.

water column for increased sensor coverage.

**5.2 Sonar system** 

$$\mathcal{S}\_{s;j} = (\dot{q}\_j - A\_{53}w\_j - A\_{55}q\_j)\mathcal{B}\_{52}^{-1} \tag{32}$$

In this case the last two terms in the joint penalty Δ , similar to that of (27) but developed for the new controls, enforce *s s*max δ δ ≤ and *s s* max δ δ≤ .
