*3.4. Trajectory Optimized*

In the stage of the node expansion, the discretized steering angles would select the steering angle from a set of twenty steering angles from −35 degrees to 35 degrees. This would result in the unnatural swerve steering of the articulated steering vehicle by noncontinuous steering actions. Moreover, the Hybrid A-star algorithm aims to produce the shortest path, causing the planned route would be very close to the obstacles. Therefore, the planned courses need to be improved regarding smoothness and safety.

Polynomial equations are often used to describe the trajectory of mobile transportation using the fifth-order and seventh-order polynomials. The polynomial trajectory is expressed by the *n*-order polynomial as follows:

$$\begin{split} p(t) &= p\_0 + p\_1 \times t + p\_2 \times t^2 + \cdots + p\_n \times t^n \\ &= \sum\_{i=0}^n p^i \times t^i \end{split} \tag{11}$$

where *p*0, *p*1, ... , *pn* are trajectory parameters. As the single polynomial cannot describe the complex trajectory, the entire trajectory could be divided into k-segment polynomials, and each segment is allocated with a certain time step as follows:

$$p(t) = \begin{cases} \left[1, t, t^2, \dots, t^n\right] \cdot p\_1 & t\_0 \le t \le t\_1 \\ \left[1, t, t^2, \dots, t^n\right] \cdot p\_2 & t\_1 \le t \le t\_2 \\ \dots & \\ \left[1, t, t^2, \dots, t^n\right] \cdot p\_k & t\_{k-1} \le t \le t\_k \end{cases} \tag{12}$$

where *k* denotes the number of segments, and *pi* = [*pi*0, *pi*1, ... , *pin*] *<sup>T</sup>* denotes the polynomial coefficients of the *i th* segment. Then, the trajectory optimization process can be transformed into an optimization problem for obtaining the feasible coefficient *p*1, *p*2, ... , *pk* that minimizes the integration of the square of the fourth derivative of position, namely, *snap*. The optimal problem needs to consider the constraints, such as the continuity at the junction of adjacent segments and the limits on the velocity and acceleration. In all, this optimal problem has been formulated as a constrained quadratic problem with equality constraints and inequality constraints in this work as follows:

$$\begin{cases} \min \quad \mathcal{J}(p) = p^T \mathbf{Q} p\\ \text{s.t.} \quad A\_{c\eta} \boldsymbol{q} = \mathbf{b}\_{c\eta} \\ \quad A\_{i\alpha\eta} \boldsymbol{q} \le \mathbf{b}\_{i\alpha\eta} \end{cases} \tag{13}$$

where the matrices *Q*, *Aeq* and *Aineq* are functions of the time allocation *δ<sup>t</sup>* - [*δt*<sup>1</sup> , ... , *δtk* ]. The equality constraint limits the states of movement, including the position, velocity, and acceleration within the segments, and ensures the continuity between the segments. Meanwhile, the inequality constraints form a trajectory corridor, which keeps a distance from the obstacles. To assign the appropriate time for each trajectory segment, the Euclidean distance between way-points is used to allocate time proportionally.

#### **4. Control Design**

In this section, we describe the design of the ATV trajectory tracking control scheme. We have divided the control framework into two parts. The first part deals with the longitudinal speed control of ATVs by adjusting the rotating speeds of the tracks. The second one is the steering motion of the ATVs by adjusting the articulation angular rate.

The control system produces feedback control actions by using an adaptive MPC algorithm. This system regulates the states of the tracking deviation system through feedback control. An error model for the tracking system has been proposed in Equation (6). To predict the system state, the MPC systems need future information about the planned trajectory to analyze the evolution of the tracking error over time. The path planning module could provide the control system with the possible disturbance on the ATVs. The path planner determines the disturbance vector *r* by the longitudinal velocity and yaw rate. *u* is a feedback control action aimed at minimizing the tracking error in the presence of disturbance.

#### *4.1. Reference Trajectory*

A path-planning method has been proposed in Section 3 to derive a smooth trajectory for ATVs. The reference states *x*, *y*, *θ*, *ψ* will be used to determine the ATVs' kinematic control. Based on the planned trajectory, we obtain the reference states *υ<sup>r</sup>* and ˙ *θr*. Planner subsystems formulate time-based reference trajectories by evaluating a given reference trajectory (*x<sup>t</sup> <sup>r</sup>*, *y<sup>t</sup> <sup>r</sup>*, *θr*, *ψr*) and its derivatives. The reference speeds *υ<sup>r</sup>* and the reference yaw rates ˙ *θ<sup>r</sup>* could be calculated as follows

$$\begin{aligned} \upsilon\_r &= \sqrt{(\dot{x}\_r^t)^2 + (\dot{y}\_r^t)^2} \\ \dot{\theta}\_r &= \frac{\dot{y}\_r^t \dot{x}\_r^t - \dot{x}\_r^t \dot{y}\_r^t}{(\dot{x}\_r^t)^2 + (\dot{y}\_r^t)^2} \end{aligned} \tag{14}$$
