**2.2. Software design**

Modular design is adopted in software structure. The software structure of computer control system is implemented using QNX SDP V6.5.0, a real-time operational system (RTOS), which is known as a real-time, high stable, and good portable OS. The software structure comprises six modules, the communicator module (TCP communicate and data transmit), GPS-IMU module (receive and process GPS-IMU signals), protocol module (parse the received data frame), tracker module (GPS tracking), controller module (including PID controller, ACC controller, and MPC controller), execute module (send PWM wave data to servos), and engine module (get the engine propeller speed) (**Figure 5**).

#### **Figure 5.** The software structure of USV system.

In execute module, there is an emergency sub-module that is used to stop the engine if no new data from ground station sub-system is received over 120 s. When new data from ground station sub-system are transferred to the on-board control sub-system, the execute module can generate PWM wave data to control the servos again.

In the software architecture, the key module is the controller module, which contains course keeping control, speed control, and GPS tracking control by using the measurements data from GPS-IMU module. The control period is 0.01 s.

### **3. USV dynamic modeling**

Originally, linear models are often used owing to their simple structure, and mainly include first-order Nomoto model [12], first-order sideslip model [13], and linearized speed and steering model [14]. The Nomoto model describes the yaw dynamics between turn rate and the rudder angle input. However, when the USV's heading undergoes a small perturbation, the sideslip angle is no longer zero. To eliminate this defect in Nomoto model without considering slipping motion, one can incorporate a first-order lag model for sideslip. The sideslip model describes the sway dynamics that relates sideslip angle to turn rate. In addition to sideslip model, the linearized speed model relates the speed increment to throttle increment input. The linearized steering model relates the sideslip velocity and turn rate to rudder angle input. These models are usually effective under the assumptions: (1) the forward speed changes very slowly [15]; (2) the USV is in *displacement area* [16]. The deficiencies of the linear models are clear: (1) it presents poor performance when describing the details of the ship dynamics [17]; (2) the parameters change greatly for different speeds of the USV. This deteriorates the control performance greatly.

In addition to linear models, there are two kinds of nonlinear models, *maneuvring models* and *sea keeping models*. Maneuvring theory assumes that the USV is moving in restricted calm water. Hence, the maneuvring models are derived for USV moving at positive speed *U* under a zerofrequency wave excitation assumption. The most famous model in maneuvring theory is the Abkowitz model [18] which adopts a nonlinear third-order truncated Taylor series expansion to approach the hydrodynamics at a nominal condition. Sea keeping theory, on the other hand, is the study of motion when there is wave excitation and the craft keeps its heading *ψ* and its speed *U* constant [18]. This introduces a dissipative force [19] known as fluid-memory effects. To estimate and identify the hydrodynamic derivatives of these nonlinear models, the forces and torque exerted on the USV need to be accurately measured. Unfortunately, this requires some strict experimental conditions that are often impossible [20].

In this section, we propose an active qLPV model to approach the hydrodynamics of USV. The advantage of the LPV model is that it is significantly simpler to be analyzed and allows the application of many linear control methods. The qLPV model is obtained by making the varying parameter of the LPV system a function of the state.

### **3.1. Active quasi-LPV modeling**

**Figure 5.** The software structure of USV system.

34 Recent Advances in Robotic Systems

generate PWM wave data to control the servos again.

GPS-IMU module. The control period is 0.01 s.

deteriorates the control performance greatly.

**3. USV dynamic modeling**

In execute module, there is an emergency sub-module that is used to stop the engine if no new data from ground station sub-system is received over 120 s. When new data from ground station sub-system are transferred to the on-board control sub-system, the execute module can

In the software architecture, the key module is the controller module, which contains course keeping control, speed control, and GPS tracking control by using the measurements data from

Originally, linear models are often used owing to their simple structure, and mainly include first-order Nomoto model [12], first-order sideslip model [13], and linearized speed and steering model [14]. The Nomoto model describes the yaw dynamics between turn rate and the rudder angle input. However, when the USV's heading undergoes a small perturbation, the sideslip angle is no longer zero. To eliminate this defect in Nomoto model without considering slipping motion, one can incorporate a first-order lag model for sideslip. The sideslip model describes the sway dynamics that relates sideslip angle to turn rate. In addition to sideslip model, the linearized speed model relates the speed increment to throttle increment input. The linearized steering model relates the sideslip velocity and turn rate to rudder angle input. These models are usually effective under the assumptions: (1) the forward speed changes very slowly [15]; (2) the USV is in *displacement area* [16]. The deficiencies of the linear models are clear: (1) it presents poor performance when describing the details of the ship dynamics [17]; (2) the parameters change greatly for different speeds of the USV. This

In addition to linear models, there are two kinds of nonlinear models, *maneuvring models* and *sea keeping models*. Maneuvring theory assumes that the USV is moving in restricted calm water. The LPV model usually refers to the linear time-varying models that possess linear model structure but have exogenous variable *w*(*t*) dependent system matrix **A** and input matrix **B** as shown in Eq. (1) [7]. While qLPV models means that the system matrix **A** and (or) input matrix **B** depend (or depends) on some state variable *x* of the system itself [8], that is, the system as Eq. (1).

$$
\dot{\mathbf{x}} = \mathbf{A}\left(\boldsymbol{\omega}(\boldsymbol{\iota})\right)\mathbf{x} + \mathbf{B}\left(\boldsymbol{\omega}(\boldsymbol{\iota})\right)\mathbf{u} \tag{1}
$$

$$\dot{\mathbf{x}} = \mathbf{A}\left(\boldsymbol{\omega}\left(\boldsymbol{t}\right), \boldsymbol{\omega}\left(\boldsymbol{t}\right)\right)\mathbf{x} + \mathbf{B}\left(\boldsymbol{\omega}\left(\boldsymbol{t}\right), \boldsymbol{\omega}\left(\boldsymbol{t}\right)\right)\mathbf{u} \tag{2}$$

That is, qLPV models are some kinds of generation of LPV models. The reason why we select the qLPV model structure to present the USV dynamics is that the nonlinearities of an USV cannot be ignored, especially when the USV is maneuvring with quickly varying velocity and course. Therefore, to denote the strong nonlinearities, the USV system can only be transferred into the qLPV form [21]:

$$
\dot{\nu} = \mathbf{A} \left( \nu \right) \nu + \mathbf{B} \nu \tag{3}
$$

where **A**(*v*) = **M**−1**N**(*v*); **u**=*T* sin*δ* cos*δ* <sup>T</sup> ; **B**=**M**−<sup>1</sup> <sup>0</sup> <sup>−</sup><sup>1</sup> *<sup>x</sup><sup>δ</sup>* 1 0 0 T .

The function **A**(*v*) in Eq. (3) can be denoted as linear combinations of the USV's velocity, that is,

$$\mathbf{A}\left(\boldsymbol{\nu}\right) = \mathbf{M}^{-1}\left(\mathbf{N}\_0 + \mathbf{N}\_1\left|\boldsymbol{\mu}\right| + \mathbf{N}\_2\left|\boldsymbol{\nu}\right| + \mathbf{N}\_3\boldsymbol{r} + \mathbf{N}\_4\left|\boldsymbol{r}\right|\right) \tag{4}$$

where

$$\mathbf{N}\_{0} = \begin{bmatrix} X\_{\boldsymbol{\mu}} & \mathbf{0} & \mathbf{0} \\ \mathbf{0} & Y\_{\boldsymbol{\nu}} & Y\_{\boldsymbol{\nu}} \\ \mathbf{0} & N\_{\boldsymbol{\nu}} & N\_{\boldsymbol{\nu}} \end{bmatrix}; \ \mathbf{N}\_{1} = \begin{bmatrix} X\_{\|\boldsymbol{\mu}\|} & \mathbf{0} & \mathbf{0} \\ \mathbf{0} & \mathbf{0} & \mathbf{0} \\ \mathbf{0} & \mathbf{0} & \mathbf{0} \end{bmatrix}; \ \mathbf{N}\_{2} = \begin{bmatrix} \mathbf{0} & \mathbf{0} & \mathbf{0} \\ \mathbf{0} & Y\_{\|\boldsymbol{\nu}\|} & \mathbf{0} \\ \mathbf{0} & N\_{\|\boldsymbol{\nu}\|} & \mathbf{0} \end{bmatrix}.$$

$$\mathbf{N}\_{3} = \begin{bmatrix} \mathbf{0} & m + X\_{\boldsymbol{\nu}} & m x\_{\boldsymbol{\mathcal{G}}} + X\_{\boldsymbol{\nu}} \\ -m & \mathbf{0} & \mathbf{0} \\ -m x\_{\boldsymbol{\mathcal{G}}} & \mathbf{0} & \mathbf{0} \end{bmatrix}; \ \mathbf{N}\_{4} = \begin{bmatrix} \mathbf{0} & \mathbf{0} & \mathbf{0} \\ \mathbf{0} & \mathbf{0} & Y\_{\|\boldsymbol{\nu}\|} \\ \mathbf{0} & \mathbf{0} & N\_{\|\boldsymbol{\nu}\|} \end{bmatrix}.$$

The qLPV model structure provides two advantages: (1) its parameters can be identified by some linear algorithm resulting in a nonlinear mathematical model; and (2) mature linear control synthesis schemes can be generalized to obtain adequate performance.

To further eliminate the unstructured model error, the active modeling technique is used to account for the unstructured factors. In the preceding work, we have proposed a control architecture as following **Figure 6**.

**Figure 6.** Active model-based control scheme.

In this architecture, the structured model can be used to design some nominal controller, while the online estimated model error is used to improve the closed loop performance of the nominal controller. To obtain the model error, we first rewrite the system Eq. (3) to follow the state-space form:

$$\begin{aligned} \dot{\boldsymbol{\nu}} &= \mathbf{A} \left( \boldsymbol{\nu} \right) \boldsymbol{\nu} + \mathbf{B} \boldsymbol{\mu} + \boldsymbol{\Delta} \boldsymbol{\nu} \\ \mathbf{z} &= \boldsymbol{\nu} + \boldsymbol{\nu}\_m \end{aligned} \tag{5}$$

where *v* is the system state vector; **A**(*v*)*v* + **B***u* is the structured system dynamics function which is shown in Eqs. (3) and (4); ∆*v* is the model error; *z* contains the measurements; and *vm* contains the measurement noise.

Then, UKF algorithm is used to estimate the model error ∆*v* using the available measurements. The UKF is an often-used nonlinear Kalman filter owing to its good performance with respect to strong nonlinearities. The main idea of the UKF algorithm is that it uses the unscented transformation (UT) to handle the nonlinear part and compute the influence of the nonlinear function on some stochastic variables. The UT provides an approach for approximating the statistics of a nonlinear transformation through a finite set of "*sigma points*":

$$z = f\left(x\right) \tag{6}$$

where *f* is a nonlinear function and *x* denotes an *n* × 1 stochastic variable with a mean of **x** ¯ and a covariance of *Px*.

To calculate the propagation statistics of *x* through *f*, that is, the mean **z** ¯ and covariance *Pz* of the output *z*, the UT uses the following steps. The complete process is illustrated in **Figure 7**.

**Figure 7.** Unscented transformation.

( ) ( ) <sup>1</sup>

The qLPV model structure provides two advantages: (1) its parameters can be identified by some linear algorithm resulting in a nonlinear mathematical model; and (2) mature linear

To further eliminate the unstructured model error, the active modeling technique is used to account for the unstructured factors. In the preceding work, we have proposed a control

In this architecture, the structured model can be used to design some nominal controller, while the online estimated model error is used to improve the closed loop performance of the nominal controller. To obtain the model error, we first rewrite the system Eq. (3) to follow the

control synthesis schemes can be generalized to obtain adequate performance.

architecture as following **Figure 6**.

**Figure 6.** Active model-based control scheme.

state-space form:

where

36 Recent Advances in Robotic Systems

01 2 3 4 *u vr r* - **A MNN N N N** *v* = + + ++ (4)

The normal UKF can be deduced as follows.

*UKF-I: Initialization*. The measurement noise vector statistics are calculated from the initial measurement:

$$\begin{aligned} \hat{\boldsymbol{\nu}}\_{0}^{\epsilon} &= E\left(\boldsymbol{\nu}\_{0}^{\epsilon}\right), \\ \boldsymbol{\mathcal{P}}\_{0} &= V \text{ar}\left(\boldsymbol{\nu}\_{0}^{\epsilon}\right) = E\left[\left(\boldsymbol{\nu}\_{0}^{\epsilon} - \hat{\boldsymbol{\nu}}\_{0}^{\epsilon}\right)\left(\boldsymbol{\nu}\_{0}^{\epsilon} - \hat{\boldsymbol{\nu}}\_{0}^{\epsilon}\right)^{\mathsf{T}}\right], \end{aligned} \tag{7}$$

where *E*[\*] denotes the mean (i.e., expectation) of [\*].

*UKF-II: Sigma points calculation*. At the *k*th time instant, using the *k*th time instant mean **v** ^ *k e* and covariance *Pk*, the sigma points are defined using a Gaussian distribution.

$$\begin{aligned} \xi\_{0,k}^{\varepsilon} &= \hat{\boldsymbol{\nu}}\_{k}^{\varepsilon}, \\ \xi\_{i,k}^{\varepsilon} &= \hat{\boldsymbol{\nu}}\_{k}^{\varepsilon} + \left(\sqrt{\left(n+\mathcal{\lambda}\right)\boldsymbol{\mathcal{P}}\_{k}}\right)\_{i}, \quad \left(\forall i \in \{1,2,\cdots,n\}\right), \\ \xi\_{i+n,k}^{\varepsilon} &= \hat{\boldsymbol{\nu}}\_{k}^{\varepsilon} - \left(\sqrt{\left(n+\mathcal{\lambda}\right)\boldsymbol{\mathcal{P}}\_{k}}\right)\_{i}. \end{aligned} \tag{8}$$

*UKF-III: Time update*. Using the sigma points, the new state is acquired:

$$\begin{split} \mathcal{T}\_{i,k|k-l}^{e} &= \mathcal{J}\_{k-l}^{e} \left( \tilde{\boldsymbol{\xi}}\_{i,k-1}^{e}, \boldsymbol{\mu}\_{k-l} \right) + \boldsymbol{\mathsf{q}}\_{k-l}, \quad \left( \forall i \in \{0, 1, \cdots, L\} \right), \\ \hat{\boldsymbol{\nu}}\_{i|k-l}^{e} &= \sum\_{\boldsymbol{\ell}=0}^{L} \boldsymbol{W}\_{i}^{m} \boldsymbol{\mathcal{Y}}\_{i,k|k-l}^{e} = \sum\_{\boldsymbol{\ell}=0}^{L} \boldsymbol{W}\_{i}^{m} \, \boldsymbol{f}\_{k-l}^{e} \left( \tilde{\boldsymbol{\xi}}\_{i,k-1}^{e}, \boldsymbol{\mu}\_{k-l} \right) + \boldsymbol{\mathsf{q}}\_{k-l}, \\ \boldsymbol{\mathsf{P}}\_{i|k-l} &= \sum\_{\boldsymbol{\ell}=0}^{L} \boldsymbol{W}\_{i}^{c} \left( \boldsymbol{\mathcal{Y}}\_{i,k|k-l}^{e} - \hat{\boldsymbol{\nu}}\_{k|k-l}^{e} \right) \left( \boldsymbol{\mathcal{Y}}\_{i,k|k-l}^{e} - \hat{\boldsymbol{\nu}}\_{k|k-l}^{e} \right)^{\top} + \boldsymbol{\mathsf{Q}}\_{k-l}. \end{split} \tag{9}$$

*UKF-IV: Measurements update*. Similarly, using the updated state **v** ^ *k* |*k* −1 *<sup>e</sup>* and **P***<sup>k</sup>* <sup>|</sup>*<sup>k</sup>* <sup>−</sup>1 in *UKF-III*, new sigma points **ξ***i*,*<sup>k</sup>* <sup>|</sup>*<sup>k</sup>* <sup>−</sup><sup>1</sup> *<sup>e</sup>* can be calculated to update the measurements.

$$\begin{split} \mathcal{X}\_{i,:\|k\|-1} &= \mathsf{H}^{c}\_{i} \left( \boldsymbol{\xi}^{\boldsymbol{e}}\_{i,:\|k\|-1} \right) + \mathsf{r}\_{k}, \quad \left( \forall i \in \{0, 1, \cdots, L\} \right), \\ \hat{\boldsymbol{z}}\_{i,\|k-1} &= \sum\_{i=0}^{L} W^{\boldsymbol{m}}\_{i} \boldsymbol{\mathcal{X}}\_{i,:\|k-1} = \sum\_{i=0}^{L} W^{\boldsymbol{m}}\_{i} \mathsf{H}^{c}\_{i} \left( \boldsymbol{\xi}^{\boldsymbol{e}}\_{i,:\|k-1\|} \right) + \mathsf{r}\_{k}, \\ \boldsymbol{\mathsf{P}}\_{\hat{\boldsymbol{z}}\_{i}} &= \sum\_{i=0}^{L} W^{\boldsymbol{c}}\_{i} \left( \boldsymbol{\mathcal{X}}\_{i,:\|k-1\|} - \hat{\boldsymbol{z}}\_{i\|k-1} \right) \left( \boldsymbol{\mathcal{X}}\_{i,:\|k-1\|} - \hat{\boldsymbol{z}}\_{i\|k-1} \right)^{\top} + \mathsf{R}\_{k}, \\ \boldsymbol{\mathsf{P}}\_{\hat{\boldsymbol{\nu}}\_{i}\boldsymbol{\xi}\_{i}} &= \sum\_{i=0}^{L} W^{\boldsymbol{c}}\_{i} \left( \boldsymbol{\xi}^{\boldsymbol{e}}\_{i,:\|k-1\|} - \hat{\boldsymbol{\nu}}^{\boldsymbol{e}}\_{i,:\|k-1\|} \right) \left( \boldsymbol{\mathcal{X}}\_{i,:\|k-1\|} - \hat{\boldsymbol{z}}\_{i\|k-1} \right)^{\top}. \end{split} \tag{10}$$

When the new measurement *zk* is obtained, the state **v***<sup>k</sup> e* is updated:

$$\begin{aligned} \boldsymbol{K}\_{k} &= \boldsymbol{P}\_{\boldsymbol{\theta}\_{k}\boldsymbol{z}\_{k}} \boldsymbol{P}\_{\boldsymbol{z}\_{k}}^{-1}, \\ \hat{\boldsymbol{\nu}}\_{k}^{c} &= \hat{\boldsymbol{\nu}}\_{k|k-1}^{c} + \boldsymbol{K}\_{k} \left( \boldsymbol{z}\_{k} - \hat{\boldsymbol{z}}\_{k|k-1} \right), \\ \boldsymbol{P}\_{k} &= \boldsymbol{P}\_{k|k-1} - \boldsymbol{K}\_{k} \boldsymbol{P}\_{\hat{\boldsymbol{z}}\_{k}} \boldsymbol{K}\_{k}^{\mathrm{T}}, \end{aligned} \tag{11}$$

where *Qk* and *R<sup>k</sup>* are the process and measurement noise covariance, respectively, which are both assumed to be known a priori. The parameter *α* is usually set within [0.0001, 1].

### **3.2. Identification results**

( )

*V E*

0 0

=

38 Recent Advances in Robotic Systems

where *E*[\*] denotes the mean (i.e., expectation) of [\*].

ˆ ,

*e e k k e e*

=

*ξ v*

0,

,

+

*III*, new sigma points **ξ***i*,*<sup>k</sup>* <sup>|</sup>*<sup>k</sup>* <sup>−</sup><sup>1</sup>

,

*e e*

*v v*

*e e*

*E*

ˆ ,

( ) ( )( )

é ù = =-- ê ú ë û

ar ˆ ˆ ,

*UKF-II: Sigma points calculation*. At the *k*th time instant, using the *k*th time instant mean **v**

( ) ( ) ( )

*n in*

*<sup>e</sup>* can be calculated to update the measurements.

*e*

is updated:

ˆ , {1,2, , } ,

*e eeee*

0 0 0000

*P v vvvv*

and covariance *Pk*, the sigma points are defined using a Gaussian distribution.

( ) ( )

l

l

= + "Î

ˆ .

*n*

*UKF-III: Time update*. Using the sigma points, the new state is acquired:

*UKF-IV: Measurements update*. Similarly, using the updated state **v**

When the new measurement *zk* is obtained, the state **v***<sup>k</sup>*

*ik k <sup>k</sup> <sup>i</sup>*

*ξ v+ P*

=- +

*ξv P*

*i nk k <sup>k</sup> <sup>i</sup>*

T

L

^ *k* |*k* −1

*<sup>e</sup>* and **P***<sup>k</sup>* <sup>|</sup>*<sup>k</sup>* <sup>−</sup>1 in *UKF-*

(7)

^ *k e*

(8)

(9)

(10)

In this section, we describe the results of two experiments: (1) full state model identification experiment; (2) active modeling enhanced qLPV model experiment. In the final, the results are analyzed and compared.

To quantitatively compare the identification errors, an index function is defined to evaluate the identification errors:

$$J\_{\rm index} = \sqrt{\frac{1}{N} \sum\_{l=1}^{N} \left[ \mathbb{1}\_{\rm ext} \left( i \right) - \mathbb{1}\_{\rm rad} \left( i \right) \right]^2} \mid \sqrt{\frac{1}{N} \sum\_{l=1}^{N} \mathbb{1}\_{\rm rad}^2 \left( i \right)}\tag{12}$$

To verify the model accuracy for different motion, experiments were conducted using a sine wave rudder angle input as shown in **Figure 8**. The quantitative comparisons are given in **Table 3**.

**Figure 8.** Sine wave rudder angle: input of the USV.

From these results, one can see that the qLPV model presents better performance for the USV system, especially for surge dynamics, for which the prediction error is <3% (no more than half of that for the two linear models). For sway dynamics, the qLPV model also provides better accuracy, but it is not as good as that for surge dynamics. This means that sway dynamics and


yaw dynamics possess stronger nonlinearity since the sway and yaw speed are usually smaller and easily influenced by external disturbances compared to surge dynamics.

**Table 3.** Modeling error for sine input.

To further reduce the model mismatch's influence and improve the estimation accuracy, the active modeling scheme is used. Using an UKF algorithm, the model error of the qLPV structured model is estimated online.

The parameters of the UKF algorithm are

$$\begin{aligned} \underline{Q} &= 10^{-2} \operatorname{diag} \left( 0, 0, 0, 16, 2, 1 \right), R = 10^{-3} \operatorname{diag} \left( 16, 2, 1 \right), \\ \alpha &= 1 \times 10^{-3}, \beta = 2, \end{aligned} \tag{13}$$

and we use the same data as that of the full state model identification experiment, that is, the throttle was set to 30%, and the rudder angle follows a sine wave with amplitude π/3. The prediction error computed by using Eq. (12) is shown in **Table 4**. Compared to the results from the qLPV model without active modeling in **Table 3**, the model accuracy improvement is significant. The active modeling enhanced qLPV model significantly reduces the prediction errors (only one-third of that for the qLPV model). This is intended to make the USV autono‐ mously adaptive to its internal and external uncertainties, that is, to achieve a robust tracking performance for time-varying unknown disturbances in the vehicle dynamics.


**Table 4.** Modeling error for active modeling enhanced qLPV model.
