*3.2. Control Design*

In this subsection, we will design two torque controllers (*u*1, *u*2) for the dynamics Equation (18) by using the dynamical tracking target (20). We see that the yaw rotation speed target *ω*˜ in the second equation of (20) is the product of the actual forward speed *v* and the relative curvature *k*(*s*(*t*)). Therefore, the controller *u*<sup>1</sup> in the first equation of (18) should be firstly considered so as to obtain the actual forward speed.

#### 3.2.1. Forward Speed Control Subsystem

At first, we consider the first equation of the dynamics model (18)

$$
\psi\_0 = \frac{-\Delta\_2 v\_0^2 + u\_1}{\Delta\_1}.\tag{22}
$$

Applying the feedback linearization method to (22) and letting

$$
u\_1 = \Delta\_1 h\_1 + \Delta\_2 v\_{0\prime}^2 \tag{23}$$

a simple control system is obtained as

$$
\vartheta\_0 = h\_1(t).
$$

Defining **<sup>X</sup>**<sup>1</sup> = (*s*1, *<sup>v</sup>*0)<sup>T</sup> and *<sup>s</sup>*<sup>1</sup> = *<sup>t</sup>* <sup>0</sup> *v*0(*ξ*)d*ξ*, the forward speed subsystem is rewritten in a matrix form as

$$
\dot{\mathbf{X}}\_1(t) = \mathbf{A}\_1 \mathbf{X}\_1(t) + \mathbf{B}\_1 h\_1(t), \tag{24}
$$

where **A**<sup>1</sup> = & 0 1 0 0 ' , and **B**<sup>1</sup> = & 0 1 ' . In this way, (24) is transformed into an error system as

$$
\dot{\mathbf{Y}} = \mathbf{A}\_1 \mathbf{Y} + \mathbf{B}\_1 h\_1(t) + \eta(t),
\tag{25}
$$

where **<sup>X</sup>**˜ <sup>1</sup> = (*s*˜1, *<sup>v</sup>*˜0)T, *<sup>s</sup>*˜1 = *<sup>t</sup>* <sup>0</sup> *<sup>v</sup>*˜0(*τ*)d*τ*, **<sup>Y</sup>** = (*y*1, *<sup>y</sup>*2)<sup>T</sup> <sup>=</sup> **<sup>X</sup>**<sup>1</sup> <sup>−</sup> **<sup>X</sup>**˜ 1, and *<sup>η</sup>*(*t*) = **<sup>A</sup>**1**X**˜ <sup>1</sup> <sup>−</sup> ˙ **X**˜ 1. It is obvious that the integral of (21) with respect to *t* from 0 to infinity is convergent. Thus, a linear quadratic performance index is introduced as

$$J = \frac{1}{2} \int\_0^{+\infty} [\mathbf{Y}^\mathrm{T}(t)\mathbf{Q}\_1\mathbf{Y}(t) + h\_1^\mathrm{T}(t)\mathbf{R}h\_1(t)]\mathrm{d}t.$$

Here, matrix **Q**<sup>1</sup> should be large weight of the forward speed error. In this way, the forward speed error is able to be small enough by using optimal control. According to the linear quadratic optimal control theory, the optimal control of forward speed error subsystem (25) is formulated as

$$h\_1(t) = -\mathbf{R}^{-1}\mathbf{B}\_1^T[\mathbf{P}\mathbf{Y} + \mathbf{b}(t)].\tag{26}$$

where **<sup>P</sup>** <sup>∈</sup> <sup>R</sup>2×<sup>2</sup> and **<sup>b</sup>**(*t*) <sup>∈</sup> <sup>R</sup><sup>2</sup> satisfy the following equations, respectively,

$$\begin{cases} -\mathbf{P}\mathbf{A}\_1 - \mathbf{A}\_1^T \mathbf{P} + \mathbf{P} \mathbf{B}\_1 \mathbf{R}^{-1} \mathbf{B}\_1^T \mathbf{P} - \mathbf{Q}\_1 = \mathbf{0},\\ \dot{\mathbf{b}} = -[\mathbf{A}\_1 - \mathbf{B}\_1 \mathbf{R}^{-1} \mathbf{B}\_1^T \mathbf{P}]^\mathrm{T} \mathbf{b} - \mathbf{P} \eta(t), \quad \mathbf{b}(+\infty) = \mathbf{0}. \end{cases}$$

Substituting (26) into (23), the controller *u*<sup>1</sup> is formulated by

$$\mu\_1 = -\Delta\_1 \mathbf{R}^{-1} \mathbf{B}\_1^\mathsf{T} [\mathbf{P} \mathbf{Y} + \mathbf{b}(t)] + \Delta\_2 v\_0^2.$$
