*2.3. Kinematic LPV Modelling*

To construct the linear parameter varying (LPV) tracking error model, we define a scheduling variable including the reference velocity and yaw-rate, expressed as *ρ*(*k*) := [*υr*, ˙ *θr*]. In the LPV model of the tracking error system, the state, control, and output variables are defined as follows:

$$\mathbf{x} = \begin{bmatrix} \mathbf{e}\_x \\ \mathbf{e}\_y \\ \mathbf{e}\_\theta \\ \mathbf{e}\_\psi \\ \gamma \end{bmatrix} \quad \mathbf{u} = \begin{bmatrix} \boldsymbol{\upsilon} \\ \boldsymbol{\dot{\gamma}} \end{bmatrix} \quad \mathbf{y} = \begin{bmatrix} \boldsymbol{e}\_x \\ \boldsymbol{e}\_y \\ \boldsymbol{e}\_\theta \\ \boldsymbol{e}\_\psi \end{bmatrix} \quad \mathbf{r} = \begin{bmatrix} \boldsymbol{\upsilon}\_r \\ \boldsymbol{\theta}\_r \end{bmatrix} \tag{5}$$

Then the tracking error model is transformed into the formulation of the LPV model:

$$
\dot{\mathbf{x}} = \mathbf{A}(\rho(k))\mathbf{x} + \mathbf{B}\_{\text{H}}(\rho(k))\mathbf{u} + \mathbf{B}\_{\text{T}}\mathbf{r} \tag{6}
$$

where

$$\begin{aligned} \mathbf{A}(\rho(k)) &= \begin{bmatrix} 0 & \Xi\_1 & 0 & 0 & 0 \\ -\Xi\_1 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & \Xi\_2 \\ 0 & 0 & 0 & 0 & 0 \end{bmatrix} \\ \mathbf{B}\_{\mathbf{n}}(\rho(k)) &= \begin{bmatrix} 1 & e\_{\mathcal{Y}} \frac{L\_r}{D} \\ \sin e\_{\theta} & -e\_{\mathcal{X}} \frac{L\_r}{D} \\ 0 & \frac{L\_r}{D} \\ 0 & -\frac{L\_f}{D} \\ 0 & 1 \end{bmatrix} \mathbf{B}\_{\mathbf{r}} = \begin{bmatrix} 1 & 0 \\ 0 & 0 \\ 0 & -1 \\ 0 & -1 \\ 0 & 0 \end{bmatrix} \end{aligned}$$

where <sup>Ξ</sup><sup>1</sup> = *<sup>υ</sup><sup>r</sup>* sin *<sup>γ</sup><sup>r</sup> <sup>D</sup>* and <sup>Ξ</sup><sup>2</sup> <sup>=</sup> *<sup>υ</sup><sup>r</sup> <sup>D</sup>* , *D* = *Lf* + *Lr*. From this LPV formulation of the trackingerror model, a polytopic representation for the error model can be expressed as

$$\mathbf{x}(k+1) = \left(\sum\_{i=1}^{2^{\varepsilon\_{\varepsilon}}} \mu\_i \mathbf{A}\_i \right) \mathbf{x}(k) + \left(\sum\_{i=1}^{2^{\varepsilon\_{\varepsilon}}} \mu\_i \mathbf{B}\_i \right) \mathbf{u} + \mathbf{B}\_{\mathbf{r}} \mathbf{r} \tag{7}$$

where *rc* is the number of scheduling component in *ρ*(*k*). The matrix *Ai* and *Bi* denotes the each polytopic vertex of the matrix *A*(*ρ*(*k*)), *Bu*(*ρ*(*k*)), defined by the extreme realization of scheduling components in *ρ*(*k*). The weighting coefficient *μ<sup>i</sup>* is used to comprehensively describe the scheduling variables *ρ*(*k*), which therefore determines the realization of *A*(*ρ*(*k*)), *Bu*(*ρ*(*k*)) in the control system. Using the available information of scheduling

variable *ρ*(*k*), we utilize the Takagi-Sugeno fuzzy method to obtain the state-space matrices of the LPV formulation [29]. The membership function *μ<sup>i</sup>* can be expressed as follows:

$$\begin{cases} \begin{aligned} M\_{\upsilon\_{r}} &= \frac{\bar{\upsilon}\_{r} - \hat{\upsilon}}{\mathfrak{d}\_{r} - \underline{\upsilon}\_{r}} & M\_{\varnothing\_{r}} = \frac{\check{\theta}\_{r} - \hat{\theta}}{\check{\theta}\_{r} - \underline{\theta}\_{r}} \\ & \mu\_{1}(\upsilon, \theta) = M\_{\upsilon\_{r}} M\_{\varnothing\_{r}} \\ & \mu\_{2}(\upsilon, \dot{\theta}) = M\_{\upsilon\_{r}} (1 - M\_{\varnothing\_{r}}) \\ & \mu\_{3}(\upsilon, \dot{\theta}) = (1 - M\_{\upsilon\_{r}}) M\_{\varnothing\_{r}} \\ & \mu\_{4}(\upsilon, \theta) = (1 - M\_{\upsilon\_{r}}) (1 - M\_{\varnothing\_{r}}) \end{aligned} \tag{8}$$

where *υ*¯*<sup>r</sup>* and *υ<sup>r</sup>* denote the upper bound and lower bound of the longitudinal speed of reference. ¯˙ *θ<sup>r</sup>* and ˙ *θ<sup>r</sup>* denote the upper bound and lower bound of the yaw rates of the reference. *<sup>υ</sup>*ˆ*<sup>r</sup>* denotes the measured longitudinal speeds of the vehicle; ˆ˙ *θ* denotes the measured yaw rate of the front unit of the ATVs.

#### **3. Trajectory Planning**

This section defines the path planner of ATVs in two stages, namely path searching and trajectory optimization, as shown in Figure 3. The environment map is divided by the grids at first. In the simulation, the ATVs are shrunk into two coupled rectangles moving on a two-dimensional map. Obstacles, Λ*i*(*i* = 1, 2, ... , *n*), are marked with a cyan color that the ATVs are not permitted to cross. The simulation has predefined the start point (*X*,*Y*, *θ*)*<sup>S</sup>* and goal point (*X*,*Y*, *θ*)*T*. For the ATVs to reach the goal point and avoid obstacles, a Hybrid A-star algorithm is proposed to generate a feasible kinematic trajectory. The planned trajectory is optimized with the minimum snap algorithm for smoothness and continuous acceleration. By interpolating the kinematic states concerning the time, the optimal trajectory could be expressed by polynomial functions. It will check whether its velocity and acceleration meet the physical limits. At the final step, the reference path is summarized in terms of a series of points attached with the kinematic states of ATVs (*xr*, *yr*, *θr*, *ψr*).

**Figure 3.** Flow chart of the trajectory planning algorithm of ATVs.

#### *3.1. Node Expansion*

Path search is used to construct a collision-free route for the ATVs from the initial position *S* to the goal position *T*. In this stage, we propose the Hybrid A-star algorithm, which expands path nodes in the map's continuous space to maintain the kinematic feasibility of the planned path. The continuous moving state is defined as (*x*, *y*, *θ*, *γ*), which refers to the position, orientation, and steering angle of the moving object. *t* is the expanded node, and its cost can be evaluated by the function *f(t)*. Based on the lowest value of the evaluation function *f(t)*, the Hybrid A-star algorithm expands the path node. It updates the evaluation function of sub-nodes associated with the current node *t* until all nodes have been traversed or the current node *t* is near the goal node *T*. Using the function *g*(*t*), we could obtain the travel cost from the start node *S* to the current node *t*. In the meantime, a heuristic function *h*(*t*) can predict the heuristic cost between the current position and the goal. The overall cost can be calculated using *f*(*t*) as follows:

$$f(t) = \mathcal{g}(t) + \mathbb{C}\_h h(t) \tag{9}$$

where *Ch* denotes the weighting coefficient of the heuristic cost. The list *L* denotes the collection of nodes for the next expansion, and the list *C* refers to the expanded node collection. The actual cost considers the moving distance from *S* to *t*. It includes additional penalties for the steering angle and steering angle increments to avoid unreasonable steering maneuvers. The actual cost *g*(*t*) for the current node can be expressed as follows

$$\mathbf{g}(t) = \mathbb{C}\_{\text{distance}} + \gamma \ast \mathbb{C}\_{\text{ster}} + (\gamma\_{\text{prev}} - \gamma) \ast \mathbb{C}\_{\text{ster\\_diff}} + \mathbb{C}\_{\text{prev}}.\tag{10}$$

*Cdistance* denotes the distance from *t* to its parent node; *Cprev* denotes the actual cost for the parent node. *γ* denotes the articulation steering angle for the current node while *γprev* denotes the steering action for the parent node. *Csteer* and *Csteer*\_*di f f* denote the weighting coefficients for the steering input and the steering input change.

#### *3.2. Heuristics Cost*

As shown in Equation (10), the actual cost from *S* to current node *t* has been evaluated by the function *g*(*t*). Since the minimum value of *g*(*t*) represents the optimal trajectory and the corresponding steering action, the steering action to the goal can be derived from the heuristic function *h*(*t*). The classical Hybrid A-star algorithm usually adopts two kinds of heuristics. The first heuristic function is the nonholonomic-without-obstacles, which only considers the kinematics of a moving object without considering the obstacles. The second approach is holonomic-with-obstacles, which ignores the kinematics of moving objects and produces a trajectory with the minimum Euclidean distance between the current position and the destination in the presence of obstacles. To obtain the optimal guidance for the current node, the algorithm adopts the maximum value of the two heuristics as the heuristic function *h*(*t*) for the current node. To expand the node from *t* to *T*, the first heuristics use the Reeds-Shepp (RS) curve, which takes into account the ATVs' kinematics. Using the grid A-star cost map as the second heuristic function enables the ATV to avoid inefficient path searches by providing the map's information to the vehicle. The maximum of both heuristics has been calculated. The value of function *h*(*t*) will be determined by the maximum of the two heuristics.
