Trajectory Planning for an Articulated Tracked Vehicle and Tracking the Trajectory via an Adaptive Model Predictive Control

Abstract

This paper focuses on the trajectory planning and trajectory tracking control of articulated tracked vehicles (ATVs). It utilizes the path planning method based on the Hybrid A-star and the minimum snap smoothing method to obtain the feasible kinematic trajectory. To overcome the highly non-linearity of ATVs, we proposed a linear-parameter-varying (LPV) kinematic tracking-error model. Then, the kinematic controller was formulated as the adaptive model predictive controller (AMPC). The simulation of the path planning algorithm showed that the proposed planning strategy could provide a feasible trajectory for the ATVs passing through the obstacles. Moreover, we compared the AMPC controller with the developed controller in four scenarios. The comparison showed that the AMPC controller achieved satisfactory tracking errors regarding the lateral position and orientation angle errors. The maximum lateral distance error by the AMPC controller has been reduced by 72.4% compared to the standard-MPC controller. The maximum orientation angle error has been reduced by 55.53%. The simulation results confirmed that the proposed trajectory planning and tracking control system could effectively perform the automated driving behaviors for ATVs.

1. Introduction

In articulated tracked vehicles, two double-tracked units are joined by an articulated mechanism. Unlike skid-steer vehicles, ATVs have articulated steering mechanisms driven by hydraulic actuators, which allow them to produce an articulation angle between the front and rear units of the ATVs and steer the front unit to the desired location [1]. ATVs have the advantage of a balanced driving force between both tracks, which results in minimal driving torque requirements in the steering maneuver compared to single and coupled tracked vehicles [2].

Research has been focused on off-road vehicles to enhance driving efficiency and ensure safety by using automated driving systems [3,4]. Several approaches have been applied to obtain a feasible kinematic trajectory for the off-ground vehicle [5]. As the articulated steering mechanism gives ATVs unique steering characteristics, the rear unit of ATVs contributes to the overall nonholonomic constraints. It is, therefore, impossible for traditional planning methods such as the RRT [6] or artificial potential field [7] to produce a feasible and smooth path for the ATVs. The Hybrid A-star algorithm could produce a smooth, kinematics feasible path for nonholonomic systems [8,9]. The Hybrid A-star is implemented in two stages, including the node search, to produce a kinematics-feasible trajectory. The second stage then locally improves the quality of the path using analytical expansion of the path.

For trajectory tracking, two types of modeling have been commonly used: kinematics-based modeling and dynamics-based modeling. Because those ATVs operate at low speeds, dynamic factors such as road-wheel load distribution and centrifugal force can be overlooked. The articulated steering vehicle (ASV) path-tracking deviation model has been extended from ordinary mobile robots in terms of speed deviation, lateral position deviation, and heading angle deviation. To simplify the automatic guidance of ASV, Nayl defined an improved path tracking model, considering the lateral displacement deviation, heading angle deviation, and curvature deviation [10]. A tracking error model, which includes both the position error and orientation error of both the front and rear units, has been applied in [11] to facilitate the control design for the rear unit of the ASV.

Based on the above path tracking error model, Ridley developed a full state feedback adaptive tracking method [12]. There are also numerous applications of complex controllers based on robust control [13], fuzzy control [14], and sliding mode control [15,16]. In addition, researchers proposed a linear switching control strategy that took advantage of the linearization of the ASV to overcome its nonlinear characteristics [17]. A simpler and more robust trajectory tracking controller, including the MPC controller, was suggested for the tracked vehicle [18]. A further advantage of the model predictive control (MPC) algorithm is that it takes constraints into account. Thus, the real-time status of the vehicle can be taken into account directly in the MPC controller. A switch controller consisting of multiple MPC controllers was proposed to account for the side angles of the ASV [19]. On the articulated vehicle platform, Kayacan implemented linear MPC, nonlinear MPC, and robust tube-based MPC algorithms for path tracking [20,21]. An articulated wheel loader achieved good tracking accuracy despite varying road curvature using the adaptive MPC method [22].

The steering characteristics of ATVs have been extensively studied in the recent research on ATVs in [23,24]. Furthermore, the design parameters regarding the steering characteristics of the ATVs were examined in [25,26]. A fuzzy-PID control system has been proposed to obtain the articulation angle of ATVs to track a predefined path [27]. After that, a closed-loop control of the steering torque of the ATV hydraulic-driven system was introduced to obtain the desired articulation angle [28]. Despite this, researchers have not studied motion control for ATVs during complex maneuvers in obstacle-filled environments. Using the trajectory provided by the path planner and optimization modules, this paper proposed a trajectory-tracking control framework based on an adaptive MPC control framework for tracking the trajectory of ATVs. The following works have been completed:

• Using the Hybrid A-star path planning method to obtain a feasible kinematic trajectory.
• Using the minimum snap method to optimize the planned trajectory and obtain the reference vehicle kinematic states.
• Designing a kinematic controller based on the AMPC control scheme to achieve robust trajectory tracking control.

This paper structure is presented as follows. In Section 2, we establish the kinematic model as well as the trajectory-tracking-error model for the ATVs. Section 3 presents the trajectory planner for ATVs based on the Hybrid A-star method and the trajectory optimized method based on the minimum snap method. Section 4 describes the two-layer trajectory-tracking controller consisting of a forward control method and an adaptive model predictive method. We discuss the simulation results of the proposed trajectory planner and the control framework in Section 5. The conclusion of this work is presented in Section 6.

2. Autonomous Articulated Vehicle System

The geometry of the articulated tracked vehicle is shown in Figure 1. The ATVs comprise two vehicles connected by articulating mechanisms and hydraulic steering actuators. Changing the articulation angle allows the ATVs to perform steering maneuvers. Additionally, the front and rear vehicle’s tracks are controlled to maintain the longitudinal speed. The motion control of ATVs is intended to guide the front and rear units of the ATVs to the reference trajectory determined by the trajectory planner. The reference trajectory is defined as $[x_r\left(t\right),y_r\left(t\right),{\theta }_r,{\psi }_r]$, where $[x_r\left(t\right),y_r\left(t\right),{\theta }_r]$ are the center of the front vehicle’s gravity, and ${\psi }_r$ denotes the orientation angle of the rear unit, respectively. In this work, the reference trajectory, namely, ${\mathit{q}}_r={[x_r\left(t\right),y_r\left(t\right),{\theta }_r\left(t\right),{\psi }_r\left(t\right)]}^T$, and the derivatives of the reference trajectory are all continuous and bound. The longitudinal velocities of the front and rear vehicles are denoted by $\upsilon$. $\dot{\theta }$ and $\dot{\psi }$ are the yaw rates of the two ATV units, respectively. Then, the state vector $\mathit{q}={[x,y,\theta ,\psi ]}^T$ denotes the ATVs’ position and orientation.

In this paper, we consider the path planning and tracking of ATVs in a structured environment, including the boundaries and obstacles described by the rectangle. The problem of achieving a viable path and accurate control can be divided into two stages, trajectory planning, and trajectory-tracking control. Several goals must be fulfilled in the trajectory planning process, such as continuous driving velocity profiles and establishing a feasible path to avoid obstacles. This planner provides a discrete and smooth reference path for the trajectory-tracking controller. Finally, the controller produces accurate velocity and steering angle for the ATVs to travel to the destination safely. Figure 2 shows the scheme of the whole work, including the path planning and the path tracking control. In the control system, the trajectory planning module provides the reference positions $X_r,Y_r,{\theta }_r,{\psi }_r$ and the kinematic reference states ${\upsilon }_r,{\dot{\theta }}_r$ to the trajectory tracking controller as the external disturbance.

2.1. Kinematic Vehicle Models

In this section, the developed kinematic model is used to capture the main feature of the kinematics of the ATVs [15], which can be expressed as follows:

$$\begin{array}{cc}\hfill \dot{x}& =\overline{\upsilon }cos\theta \hfill \\ \hfill \dot{y}& =\overline{\upsilon }sin\theta \hfill \\ \hfill \dot{\theta }& =\frac{\overline{\upsilon }sin\gamma +L_r\dot{\gamma }}{L_{f}cos\gamma +L_r}\hfill \\ \hfill \dot{\psi }& =\frac{\overline{\upsilon }sin\gamma -L_f\dot{\gamma }}{L_{f}cos\gamma +L_r}\hfill \end{array}$$

(1)

where the variables x and y denote the coordinate of the geometry center of the front unit of the ATVs; $\theta$ and $\psi$ denote the orientation angle of two parts of ATVs, respectively. $\gamma$ and $\dot{\gamma }$ denote the articulate angle and the articulate angle rate. The difference between the orientation angle of two units is defined as the articulation angle $\gamma$. To maintain the safety of the ATVs in the steering process, the steering control action, namely articulation angle and articulation angle rate, should be less than the maximum value.

2.2. Tracking Error Dynamics Model

To obtain the tracking error between the vehicle and the reference trajectory, we define the variable of tracking error:

$${\mathit{q}}_{\mathit{e}}=\mathit{q}-{\mathit{q}}_{\mathit{r}}$$

(2)

where $\mathit{q}$ denotes the position and orientation of the ATVs, and ${\mathit{q}}_{\mathit{r}}$ indicates the position and direction of the reference point on the desired path. Both $\mathit{q}$ and ${\mathit{q}}_{\mathit{r}}$ are expressed in the earth-fixed frame. The tracking error ${\mathit{q}}_{\mathit{e}}$ should, however, be expressed in the vehicle-fixed frame to benefit from the computation of the kinematic controller. As a result, we use an orthogonal rotation matrix to translate the vehicle motion from the earth-fixed frame to a vehicle-fixed frame. Based on the kinematic parameter of ATVs, the transformation can be expressed as follows:

$$\begin{array}{c}\hfill {\mathit{q}}_{\mathit{e}}=\left[\begin{array}{c}{e}_x\\ e_y\\ e_{\theta }\\ e_{\psi }\end{array}\right]=\left[\begin{array}{cccc}cos{\theta }_r& sin{\theta }_r& 0& 0\\ -sin{\theta }_r& cos{\theta }_r& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]\left[\begin{array}{c}x-x_r\\ y-y_r\\ \theta -{\theta }_r\\ \psi -{\psi }_r\end{array}\right]\end{array}$$

(3)

where $e_x$ and $e_y$ denote the position distance deviation projecting on the longitudinal and lateral directions, respectively. $e_{\theta }$ and $e_{\psi }$ denote the orientation error of two units, respectively. This work presents a kinematic controller to propel the ATVs to follow a predefined or planned trajectory while maintaining the vehicle states within physical limits. In terms of the position deviation and orientation deviation, we propose the following differential equations as follows:

$$\begin{array}{c}\hfill \left\{\begin{array}{c}{\dot{e}}_x=\dot{\theta }{e}_y+\upsilon cos{e}_{\theta }-{\phi }_r\hfill \\ {\dot{e}}_y=-\dot{\theta }{e}_x-\upsilon sin{e}_{\theta }\hfill \\ {\dot{e}}_{\theta }=\dot{\theta }-\dot{{\theta }_r}\hfill \\ {\dot{e}}_{\psi }=\dot{\psi }-\dot{{\psi }_r}\hfill \end{array}\right.\end{array}$$

(4)

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 $\rho \left(k\right):=[{\upsilon }_r,{\dot{\theta }}_r]$. In the LPV model of the tracking error system, the state, control, and output variables are defined as follows:

$$\mathit{x}=\left[\begin{array}{c}{e}_x\\ e_y\\ e_{\theta }\\ e_{\psi }\\ \gamma \end{array}\right]\mathit{u}=\left[\begin{array}{c}\upsilon \\ \dot{\gamma }\end{array}\right]\mathit{y}=\left[\begin{array}{c}{e}_x\\ e_y\\ e_{\theta }\\ e_{\psi }\end{array}\right]\mathit{r}=\left[\begin{array}{c}{\upsilon }_r\\ {\dot{\theta }}_r\end{array}\right]$$

(5)

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

$$\dot{\mathit{x}}=\mathit{A}\left(\rho \left(k\right)\right)\mathit{x}+{\mathit{B}}_{\mathit{u}}\left(\rho \left(k\right)\right)\mathit{u}+{\mathit{B}}_{\mathit{r}}\mathit{r}$$

(6)

where

$$\begin{array}{cc}\hfill \mathit{A}\left(\rho \right(k\left)\right)=& \left[\begin{array}{ccccc}0& {\Xi }_1& 0& 0& 0\\ -{\Xi }_1& 0& 0& 0& 0\\ 0& 0& 0& 0& {\Xi }_2\\ 0& 0& 0& 0& 0\end{array}\right]\hfill \\ \hfill {\mathit{B}}_{\mathit{u}}\left(\rho \left(k\right)\right)=& \left[\begin{array}{cc}1& e_y\frac{L_r}{D}\\ sin{e}_{\theta }& -e_x\frac{L_r}{D}\\ 0& \frac{L_r}{D}\\ 0& -\frac{L_f}{D}\\ 0& 1\end{array}\right]{\mathit{B}}_{\mathit{r}}=\left[\begin{array}{cc}1& 0\\ 0& 0\\ 0& -1\\ 0& -1\\ 0& 0\end{array}\right]\hfill \end{array}$$

where ${\Xi }_1=\frac{{\upsilon }_{r}sin{\gamma }_r}{D}$ and ${\Xi }_2=\frac{{\upsilon }_r}{D}$, $D=L_f+L_r$. From this LPV formulation of the tracking-error model, a polytopic representation for the error model can be expressed as

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

(7)

where $r_c$ is the number of scheduling component in $\rho \left(k\right)$. The matrix ${\mathit{A}}_{\mathit{i}}$ and ${\mathit{B}}_{\mathit{i}}$ denotes the each polytopic vertex of the matrix $\mathit{A}\left(\rho \left(k\right)\right),{\mathit{B}}_{\mathit{u}}\left(\rho \left(k\right)\right)$, defined by the extreme realization of scheduling components in $\rho \left(k\right)$. The weighting coefficient ${\mu }_i$ is used to comprehensively describe the scheduling variables $\rho \left(k\right)$, which therefore determines the realization of $\mathit{A}\left(\rho \left(k\right)\right),{\mathit{B}}_{\mathit{u}}\left(\rho \left(k\right)\right)$ in the control system. Using the available information of scheduling variable $\rho \left(k\right)$, we utilize the Takagi-Sugeno fuzzy method to obtain the state-space matrices of the LPV formulation [29]. The membership function ${\mu }_i$ can be expressed as follows:

$$\begin{array}{c}\hfill \left\{\begin{array}{cc}\hfill & M_{{\upsilon }_r}=\frac{{\overline{\upsilon }}_r-\widehat{\upsilon }}{{\overline{\upsilon }}_r-{\underline{\upsilon }}_r}{M}_{{\dot{\theta }}_r}=\frac{{\overline{\dot{\theta }}}_r-\widehat{\dot{\theta }}}{{\overline{\dot{\theta }}}_r-{\underline{\dot{\theta }}}_r}\hfill \\ \hfill & {\mu }_1(\upsilon ,\dot{\theta })=M_{{\upsilon }_r}{M}_{{\dot{\theta }}_r}\hfill \\ \hfill & {\mu }_2(\upsilon ,\dot{\theta })=M_{{\upsilon }_r}(1-M_{{\dot{\theta }}_r})\hfill \\ \hfill & {\mu }_3(\upsilon ,\dot{\theta })=(1-M_{{\upsilon }_r})M_{{\dot{\theta }}_r}\hfill \\ \hfill & {\mu }_4(\upsilon ,\dot{\theta })=(1-M_{{\upsilon }_r})(1-M_{{\dot{\theta }}_r})\hfill \end{array}\right.\end{array}$$

(8)

where ${\overline{\upsilon }}_r$ and ${\underline{\upsilon }}_r$ denote the upper bound and lower bound of the longitudinal speed of reference. ${\overline{\dot{\theta }}}_r$ and ${\underline{\dot{\theta }}}_r$ denote the upper bound and lower bound of the yaw rates of the reference. ${\widehat{\upsilon }}_r$ denotes the measured longitudinal speeds of the vehicle; $\widehat{\dot{\theta }}$ 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, ${\Lambda }_i(i=1,2,\dots ,n)$, are marked with a cyan color that the ATVs are not permitted to cross. The simulation has predefined the start point ${(X,Y,\theta )}_S$ and goal point ${(X,Y,\theta )}_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 $(x_r,y_r,{\theta }_r,{\psi }_r)$.

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,\theta ,\gamma )$, 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\left(t\right)$, we could obtain the travel cost from the start node S to the current node t. In the meantime, a heuristic function $h\left(t\right)$ can predict the heuristic cost between the current position and the goal. The overall cost can be calculated using $f\left(t\right)$ as follows:

$$f\left(t\right)=g\left(t\right)+C_{h}h\left(t\right)$$

(9)

where $C_h$ 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\left(t\right)$ for the current node can be expressed as follows

$$g\left(t\right)=C_{distance}+\gamma \ast C_{steer}+({\gamma }_{prev}-\gamma )\ast C_{steer\_diff}+C_{prev}.$$

(10)

$C_{distance}$ denotes the distance from t to its parent node; $C_{prev}$ denotes the actual cost for the parent node. $\gamma$ denotes the articulation steering angle for the current node while ${\gamma }_{prev}$ denotes the steering action for the parent node. $C_{steer}$ and $C_{steer\_diff}$ 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\left(t\right)$. Since the minimum value of $g\left(t\right)$ represents the optimal trajectory and the corresponding steering action, the steering action to the goal can be derived from the heuristic function $h\left(t\right)$. 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\left(t\right)$ 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\left(t\right)$ will be determined by the maximum of the two heuristics.

3.3. Analytical Expansion

The discretized control actions achieve the expansion of the node during the forward node search. While using the optimal steering action, the moving object may not be able to precisely reach the predefined position and orientation of the goal node T. As a result, analytic expansions based on the RS curve would be used to guide the node search near the goal. The RS curve would also be checked for collision with the obstacles along the way. When the analytical expansion is employed, the algorithms perform an analysis that looks for an RS curve to the goal node T before conducting the node search from the node t. The node search would be terminated if the algorithm could find a collision-free path to the destination based on the RS curve. The RS curve would be augmented to the searched path, and the overall path planning from t to T is accomplished.

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{array}{cc}\hfill p\left(t\right)=& p_0+p_1\times t+p_2\times t^2+\cdots +p_n\times t^n\hfill \\ \hfill =& \sum _{i=0}^n{p}^i\times t^i\hfill \end{array}$$

(11)

where $p_0,p_1,\dots ,p_n$ 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:

$$\mathit{p}\left(t\right)=\left\{\begin{array}{c}\left[1,t,t^2,\cdots ,t^n\right]·p_1{t}_0\le t\le t_1\hfill \\ \left[1,t,t^2,\cdots ,t^n\right]·p_2{t}_1\le t\le t_2\hfill \\ \cdots \hfill \\ \left[1,t,t^2,\cdots ,t^n\right]·p_k{t}_{k-1}\le t\le t_k\hfill \end{array}\right.$$

(12)

where k denotes the number of segments, and $p_i={[p_{i0},p_{i1},\dots ,p_{in}]}^T$ 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,\dots ,p_k$ 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:

$$\left\{\begin{array}{cc}min\hfill & \mathcal{J}\left(p\right)=p^T\mathit{Q}p\hfill \\ \mathit{s}.\mathit{t}.\hfill & {\mathit{A}}_{eq}\mathit{q}={\mathit{b}}_{eq}\hfill \\ \hfill & {\mathit{A}}_{ineq}\mathit{q}\le {\mathit{b}}_{ineq}\hfill \end{array}\right.$$

(13)

where the matrices $\mathit{Q}$, ${\mathit{A}}_{eq}$ and ${\mathit{A}}_{ineq}$ are functions of the time allocation ${\delta }_{\mathit{t}}\triangleq [{\delta }_{t_1},\dots ,{\delta }_{t_k}]$. 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 $\mathit{r}$ by the longitudinal velocity and yaw rate. $\mathit{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,\theta ,\psi$ will be used to determine the ATVs’ kinematic control. Based on the planned trajectory, we obtain the reference states ${\upsilon }_r$ and ${\dot{\theta }}_r$. Planner subsystems formulate time-based reference trajectories by evaluating a given reference trajectory ($x_r^t,y_r^t,{\theta }_r,{\psi }_r$) and its derivatives. The reference speeds ${\upsilon }_r$ and the reference yaw rates ${\dot{\theta }}_r$ could be calculated as follows

$$\begin{array}{cc}\hfill {\upsilon }_r& =\sqrt{{\left({\dot{x}}_r^t\right)}^2+{\left({\dot{y}}_r^t\right)}^2}\hfill \\ \hfill {\dot{\theta }}_r& =\frac{{\ddot{y}}_r^t{\dot{x}}_r^t-{\ddot{x}}_r^t{\dot{y}}_r^t}{{\left({\dot{x}}_r^t\right)}^2+{\left({\dot{y}}_r^t\right)}^2}\hfill \end{array}$$

(14)

4.2. Adaptive MPC Controller

The path tracking of ATVs is controlled using the adaptive MPC algorithm, which adopts the LPV formulation instead of the non-linear model of the tracking error. Based on the information about scheduling variables, the LPV model in Equation (6) updates its state-space models. As defined in Section 3, $\rho =\left[{\upsilon }_r{\dot{\theta }}_r\right]$ is used as the scheduling variable based on the reference path given by the path planner. Using the Equations (7) and (8), the system matrices ${\mathit{A}}_k$ and ${\mathit{B}}_{\mathit{u}}k$ can be calculated at any instant k. Throughout the prediction horizon, the adaptive MPC controller could accurately predict the evolution of vehicle states. Thus, the LPV tracking error model in Equation (6) enables the MPC controller to balance computing complexity and efficiency.

To obtain a feasible control action $\mathit{u}$, the MPC controller must solve a quadratic optimization problem at each instant k. The values of past states ${\mathit{x}}_k$, the past control action ${\mathit{u}}_{k-1}$ and the disturbance vector ${\mathit{r}}_k$ are available to predict the system states ${\mathit{x}}_{k+1}$ based on the related matrix coefficients ${\mathit{A}}_k$ and ${\mathit{B}}_{\mathit{u}}k$. The optimization problem can be formulated as follows:

$$\begin{array}{c}\hfill \begin{array}{cc}\hfill & \underset{\Delta \mathit{U}}{min}{\mathrm{J}}_k=\sum _{i=1}^{H_p-1}({\mathit{x}}_{k+i}^T\mathit{Q}{\mathit{x}}_{k+i}+{\Delta {\mathit{u}}_k}^T\mathit{R}\Delta {\mathit{u}}_k)\hfill \\ \hfill & \mathrm{s}.\mathrm{t}.\hfill \\ \hfill & {\mathit{x}}_{k+i+1}={\mathit{x}}_{k+i}+({\mathit{A}}_k{\mathit{x}}_{k+i}+{\mathit{B}}_{\mathit{u}}k{\mathit{u}}_{k+i}+{\mathit{B}}_{\mathit{r}}{\mathit{r}}_{k+i})dt\hfill \\ \hfill & {\mathit{u}}_{k+i}={\mathit{u}}_{k+i-1}+\Delta {\mathit{u}}_{k+i}\hfill \\ \hfill & \Delta \mathit{U}\in \Delta \Pi \hfill \\ \hfill & \mathit{U}\in \Pi \hfill \\ \hfill & \overline{\mathit{x}}={\widehat{\mathit{x}}}_k\hfill \end{array}\end{array}$$

(15)

where $\mathit{x}={\left[e_x{e}_y{e}_{\theta }{e}_{\psi }\gamma \right]}^T$ is the state vector for the nominal system; ${\mathit{x}}_k$ denotes the estimated state vector at the time instant k. $\mathit{r}={\left[{\upsilon }_r{\dot{\theta }}_r\right]}^T$ is the disturbance vector given by the proposed path planner. $H_p$ denotes the prediction horizon. $\mathit{Q}\in {\mathcal{R}}^{5\times 5}$ and $\mathit{R}\in {\mathcal{R}}^{2\times 2}$ are semi-positive diagonal weighting coefficients for the state and control action increments to obtain a convex cost function, respectively. $\mathit{u}=\left[\upsilon \dot{\gamma }\right]$ denotes the vector of the control action. $\Delta \mathit{u}$ denotes the vector of the increment of the control actions. $\mathit{U}$ and $\Delta \mathit{U}$ denote the sequence of the control actions and their increments through the prediction horizon. $\Pi$ and $\Delta \Pi$ denote the physical limit on the articulation angular rate and its increments, respectively.

If the optimization problem in Equation (15) could be successfully solved, then a sequence of control input increments can be obtained as $\Delta \mathit{U}={[\Delta {\mathit{u}}_k\Delta {\mathit{u}}_{k+1}\dots \Delta {\mathit{u}}_{t+H_p-1}]}^T$. Based on the past control action ${\mathit{u}}_{k-1}$, the control action at the current time instant k is the summation of the past control action ${\mathit{u}}_{k-1}$ and the control increment $\Delta u_k$ from the sequence $\Delta \mathit{U}$ as follows:

$${\mathit{u}}_k={\mathit{u}}_{k-1}+\Delta u_k$$

(16)

4.3. Track-Speed Control

Based on the control action ${\mathit{u}}_k$ obtained by Equation (16), the longitudinal control of ATVs is performed by adjusting the tracks of both front and rear unit of ATVs. To achieve the desired longitudinal speed and avoid the excessive track slip, the speeds difference of different tracks, $[{\upsilon }_{fl}^r,{\upsilon }_{fr}^r,{\upsilon }_{rl}^r,{\upsilon }_{rr}^r]$ can be obtained by the calculation method developed in [15,30]:

$$\begin{array}{cc}\hfill {\upsilon }_{fl}^r& =\upsilon +\frac{\upsilon \sin \gamma }{2(L_f+L_r\cos \gamma )}-\frac{\upsilon \dot{\gamma }B}{2(L_f+L_r\cos \gamma )}\hfill \\ \hfill {\upsilon }_{fr}^r& =\upsilon -\frac{\upsilon \sin \gamma }{2(L_f+L_r\cos \gamma )}+\frac{\upsilon \dot{\gamma }B}{2(L_f+L_r\cos \gamma )}\hfill \\ \hfill {\upsilon }_{rl}^r& =\upsilon cos\gamma +\frac{\upsilon L_{r}sin{\gamma }^2}{2(L_f+L_r\cos \gamma )}+\frac{\upsilon Bsin\gamma }{2(L_f+L_{r}cos\gamma )}+\frac{\dot{\gamma }{L}_r(2L_{f}sin\gamma -Bcos\gamma )}{2(L_f+L_{r}cos\gamma )}\hfill \\ \hfill {\upsilon }_{rr}^r& =\upsilon cos\gamma +\frac{\upsilon L_{r}sin{\gamma }^2}{2(L_f+L_{r}cos\gamma )}-\frac{\upsilon Bsin\gamma }{2(L_f+L_{r}cos\gamma )}+\frac{\dot{\gamma }{L}_r(2L_{f}sin\gamma +Bcos\gamma )}{2(L_f+L_{r}cos\gamma )}\hfill \end{array}$$

(17)

where $\gamma$ and $\dot{\gamma }$ denote the articulation angle and articulation angular rate. B denotes the width of the front unit of ATVs. ${\upsilon }_{fl}^r$ and ${\upsilon }_{fr}^r$ denote the left and right tracks longitudinal speeds of the front vehicle, and ${\upsilon }_{rl}^r$ and ${\upsilon }_{rr}^r$ denote the linear speeds of tracks of the rear vehicle. The velocities of four tracks of the articulated vehicle are obtained from Equation (17), which ensures that the longitudinal speed of the front unit of ATVs is equal to the desired longitudinal velocity given by the adaptive MPC controller. It is worth noting that the track speeds given by Equation (17) could not deal with the lateral slippage of the tracks well. Therefore, the lateral motion of the vehicle may occur in the steering maneuver that causes the lateral tracking error.

5. Simulation and Discussion

5.1. Simulation Setup

This section evaluates the ATVs’ path planning and path tracking algorithms on a simulation platform by MATLAB/Simulink and Recurdyn. Multi-body dynamics software Recurdyn features a high-speed track module. The virtual ATV model in Recurdyn is shown in Figure 4. The parameters of this virtual model are based on reality. This virtual model uses parameters derived from reality. The proposed AMPC controller is evaluated for its tracking performance on this virtual model. The simulation platform’s structure is illustrated in Figure, whose parameters are given in

Table 1. Kinematic parameters of the articulated tracked vehicle model.

Symbol	Description	Value	Unit
B	Width of ATVs	2.1	[m]
D	Length of ATVs	4.8	[m]
$L_f$	Distance from the hitch point to front unit	2.6	[m]
$L_f$	Distance from the hitch point to rear unit	2.2	[m]
$\gamma$	Articulation angle	[−0.75, 0.75]	[rad]
$\dot{\gamma }$	Articulation angular rate	[−0.18, 0.18]	[rad/s]
$\upsilon$	Vehicle longitudinal speed	[−1, 4]	[m/s]

.

We conducted the simulation on the MATLAB 2022a platform to verify the proposed path planning method. The algorithm is implemented in Matlab programming language. We designed two maps with different obstacles. The ATV model was configured with two rectangles, whose sizes are $2.5\times 2$ m and $2\times 2$ m. The driving speed of the ATVs was set to 2.5 m/s. The parameters of the Hybrid A-star planner and the trajectory optimization are listed in

Table 2. Parameters of the trajectory planning and optimization.

Description	Value	Unit
Minimum turn radius	10.4	[m]
Maximum velocity	5	[m/s]
Maximum acceleration	2	[m/s${}^2$]
Maximum steering angle	0.5	[rad]
Maximum steering rate	0.15	[rad/s]
Grid resolution in distance	2	[m]
Grid resolution in yaw angle	15	[degree]
Motion step size	1	[m]
Number of steering angle candidate	20
Steer angle change weighting coefficient	2
Steer angle weighting coefficient	1
Heuristic weighting coefficient	2

. The proposed method (Method 2) was compared to the original Hybrid A-star method (Method 1). Method 1 also considers the kinematic characteristics of ATVs and implements the node search by the discretized steer angles. At the same time, the Reed-Shepp (RS) curve is not adopted by Method 1 in the path search. Moreover, the heuristic function of Method 1 is the Euclidean distance from the current node and the destination.

The simulation results of the proposed AMPC algorithm were compared with the fuzzy control and MPC control published in [27,31,32]. The performance of the path-tracking controller could be mainly determined by the lateral position error and the orientation angle error of the front unit with respect to the reference path.

Four conditions were considered in the co-simulation of the path-tracking controllers. In the first condition, the ATV was controlled to track an arc of 25 m radius with a longitudinal velocity of 0.56 m/s. The second condition was to follow a path consisting of three circles with 30 m, 20 m, and 40 m radius, respectively. The longitudinal speed was set as 3 m/s. In the third condition, the reference path is a mixed path with three straight lines and two arcs of 20 m radius. The vehicle was set to move at a speed of 4 m/s. Finally, the fourth condition was to track a path generated by the Hybrid A-star planning method proposed in Section 3. The proposed AMPC controller was compared with the standard—MPC controller in this condition. In the above simulations, the parameters of the proposed controller were fixed and presented in

Table 3. Parameters of the trajectory tracking AMPC controller.

Symbol	Description	Value
$T_s$	The sample time of controller	0.2 [s]
$H_p$	Length of the prediction horizon	10
$H_c$	Length of the control horizon	5
$\mathit{Q}$	Weighting coefficient for states	diag(0.5 0.5 1 0.1 0)
$\mathit{R}$	Weighting coefficient for control input	diag(0.1 0.2)
$\mathit{P}$	Terminal cost coefficient	diag(0.1 0.1 1 1 0)

.

5.2. Simulation of Path Planning

The comparisons of the two simulation results are illustrated in Figure 5a,b. The solid line denotes the results of Method 1, and the dash-dot line indicates the proposed Method 2. As shown in Figure 5a, both path planning methods could generate the path to the goal. A significant improvement can be observed as the path generated by Method 1 contained non-smooth segments, while the planned path of Method 2 is much smoother and contains fewer switches of turning direction. Moreover, in Method 1, the planning path is close to the obstacles when the ATVs try to turn between the obstacles, as shown in Figure 5b. On the contrary, the path by Method 2 could be in the middle of the obstacles to avoid unnecessary turns when crossing the corridor between the obstacles.

Table 4. Comparison of original method and the proposed method.

Map	Method 1				Method 2
	Curvature 1	Number 2	Length 3	Time 4	Curvature	Number	Length	Time
Map A	0.095	4	88.46	15.5	0.059	1	96.06	23.3
Map B	0.098	5	128.94	26.4	0.096	2	116.09	40.6

¹ Curvature denotes the maximum curvature of the planned path. ² Number denotes the number of the steering direction change. ³ Length denotes the overall length of the planned path. ⁴ Time denotes the computation time of the planning method to obtain the planned trajectory.

summarizes the comparison between Method 1 and Method 2 regarding the maximum curvature, path length, the number of steering direction changes, and execution time. The higher values indicate higher steering instability, a longer driving path, and a higher computation cost to find the goal. From the comparison, Method 2 generates a path with more minor curvatures than Method 1. Moreover, the path length of Method 2 is longer in map A, but shorter in map B, although the difference between the two methods is not significant (88.46 versus 96.06 and 128.94 versus 116.09, respectively). In both conditions, the proposed method’s computation time are both longer than Method 1 because the candidate RS curves must be computed and checked for the collision in Method 2. Whereas the benefit of the RS curve is the much less number of the steering change in Method 2, as the RS curve could simplify the process of the path forward search.

5.3. Simulation of the Trajectory Tracking

5.3.1. Simulation Result of Case 1

In Case 1, the ATV is controlled to follow the curved path with a radius of 25 m, and the ATV is assumed to be positioned at the initial point. In the previous research [27], the fuzzy control system was utilized to guide the ATV to follow an arc. The simulation result of the fuzzy control and the proposed AMPC control have been presented in Figure 6, Figure 7 and Figure 8. The maximum lateral error of the AMPC controller is almost the same as that of the fuzzy-PID controller, as shown in Figure 7. Moreover, the AMPC controller achieves a minor orientation deviation compared to the fuzzy-PID controller, as shown in Figure 8. The maximum orientation error in the AMPC controller has been reduced by 81.84% compared to the fuzzy-PID method. Thus, the trajectory generated by the AMPC controller is closer to the predefined path than the fuzzy-PID method, as shown in Figure 6.

5.3.2. Simulation Result of Case 2

Case 2 consists of three circle paths to test the tracking performance of the ATV for the continuous steering mode. In the previous research [31], the standard-MPC controller was proposed for path tracking of autonomous articulated vehicles. The path-tracking error model of standard-MPC is based on lateral displacement, orientation, and curvature errors. The curvature error is dedicated to the circular path with a constant radius. As our work has not included the curvature error in the path-tracking model, we have compared the lateral position error and orientation error of the AMPC method with that of the standard-MPC.

The reference path and the trajectory produced by the standard-MPC and the AMPC are presented in Figure 9. The trajectory of the AMPC is closer to the defined path than the standard-MPC. The position and orientation errors produced by the standard-MPC and the AMPC have been illustrated in Figure 10 and Figure 11. The AMPC method has achieved better performance of the lateral position error than that of the standard-MPC. The maximum position error of the standard-MPC is 2 m, while the maximum position error of the AMPC is 0.67 m. In addition, the position error response of the AMPC converges to zero at the final, while the standard-MPC retains a significant position error. The standard-MPC produces a more minor orientation angle error than the AMPC controller. The maximum orientation errors of the standard-MPC and the AMPC are 0.002 rad and 0.067 rad, respectively. The reason may be due to the unavoidable skid of the articulated tracked vehicles compared to the articulated vehicle with tires. Nevertheless, the orientation error caused by the AMPC is acceptable for the ATV in practical operation.

5.3.3. Simulation Result of Case 3

Case 3 consists of straight lines and arcs with a radius of 20 m. The previous research in [32] compared the tracking performance between the switch-MPC and the nonlinear MPC (NMPC) on the path tracking of the articulated mining vehicle with tires. Although the articulated vehicle of the research [32] differs from the ATV, the results in [32] have significant value and are worthy of reference.

Figure 12 illustrates the articulation angle response of three controllers. The maximum articulation angle response of the switch-MPC, the NMPC, and the AMPC controller is 0.5589 rad, 0.3940 rad, and 0.272 rad, respectively. Both the articulation angle response of the NMPC and the AMPC controllers exhibit less overshoot and change smoothly compared to the switch-MPC controller. Figure 13 illustrates the lateral position errors of the controllers. The maximum position errors by the switch-MPC, NMPC, and AMPC controller reached 0.7217 m, 0.0874 m, and 0.192 m, respectively. The ultimate position error generated by the AMPC controller was reduced by 73.4 % compared to the switch-MPC controller. Figure 14 presents the orientation errors of all controllers. The maximum orientation errors are 0.1458 rad, 0.0461 rad, and 0.0392 rad for the switch-MPC controller, the NMPC controller, and the AMPC controller, respectively. The maximum orientation error of the AMPC has been reduced by 73.11% compared to the switch-MPC controller and by 14.97 % with respect to the NMPC controller.

According to the research [32], the maximum computation times for the NMPC controller and the switch-MPC controller are 0.014 s, and 0.04 s, respectively. As the AMPC control system is constructed in the Matlab/Simulink, the profile report of the Simulink run-time indicates that the proposed controllers have been invoked 200 times during the whole simulation time of 40 s. The overall computation time of the controller is 0.399 s. The average computation time of the AMPC controller is approximately 0.002 s at each time step. The average computation time of the AMPC is much less than that of both the switch-MPC controller and the NMPC controller.

5.3.4. Simulation Result of Case 4

To evaluate the reliability of the AMPC algorithm to track a non-uniform trajectory given by the proposed path planner, we conducted a simulation where the ATV is controlled to follow the non-uniform trajectory with varying curvature.

The simulation results are presented in Figure 15, Figure 16, Figure 17, Figure 18 and Figure 19. The vehicle is set to move at 3 m/s. As shown in Figure 15, both the standard-MPC and the AMPC controllers could drive the ATV to follow the given path, while the AMPC controller causes less offset from the reference path compared to the standard-MPC controller. Figure 16 illustrates the articulation angle of both controllers. The ultimate articulation angle of the standard-MPC and the AMPC controller reached 0.611 rad and 0.532 rad, respectively. The maximum articulation angle of the standard-MPC is more than the limit on the articulation angle. Moreover, the articulation angle response of the AMPC controller presents less overshoot compared to the standard-MPC controller. Figure 17 illustrates the articulation angle rate of both controllers. The response of the AMPC controller exhibits more drastic changes when compared to the standard-MPC controller. At the same time, the AMPC controller outputs the articulation angle rate in advance to resist the disturbance, which could reduce the tracking error of the ATV. Figure 18 depicts the lateral tracking errors of the AMPC and the standard-MPC controller. The tracking error of the AMPC controller maintains much less than that of the MPC controller through the tracking process. Moreover, the maximum lateral tracking error of the ATV approached 0.832 m and 3.015 m by the AMPC and the standard-MPC controller, respectively. The number of maximum position errors by the AMPC controller was reduced by 72.4% compared to the standard-MPC controller. Figure 19 illustrates the orientation angle error of both controllers. The orientation error of the AMPC controller is also less than the standard-MPC controller along the time. The maximum orientation error of the AMPC controller and the standard-MPC controller is 0.125 rad and 0.269 rad, respectively. The maximum value of the orientation angle error by the AMPC controller was reduced by 53.53% compared to the standard-MPC controller.

6. Conclusions

To enhance the ability of ATVs to drive in a complex environment, we apply the Hybrid A-star method to plan a safe trajectory. Moreover, the planned path was optimized to ensure smoothness and continuity. Comparing the proposed path planner with the original Hybrid A-star method shows that the planner could generate a feasible trajectory with minimum steering direction change. Numerous studies have applied the MPC algorithm and verified its effectiveness for path-tracking control of the articulated vehicle. To achieve the trajectory tracking of the articulated tracked vehicle to follow the planned path, we propose an adaptive model predictive control (AMPC) control method that is based on the time-varying tracking error system. We obtain the following results and conclusions by comparing the AMPC controller with the previously developed fuzzy and MPC controller.

Firstly, the AMPC controller could rapidly track the reference path compared to the fuzzy-PID controller. The AMPC controller also achieves a minor orientation angle error. Secondly, the AMPC controller could achieve more minor tracking errors than the standard-MPC method. Thirdly, the tracking accuracy of the AMPC method is still inferior to the NMPC method, while the AMPC method has the advantage of computation efficiency.

From the above analysis, the main contributions of this work could be summarized as follows:

• Although the adaptive model predictive control algorithm has been applied in the path-tracking of the mobile robot, its application in the articulation vehicle is not mature. The MPC algorithm has yet to be applied in the path-tracking control of the articulated tracked vehicle. Thus, our work has extended the application of the MPC algorithm in the field of ATVs.
• The ATVs have unique steering characteristics compared to the skid-steering tracked vehicles. The path tracking of the ATVs also needs to consider its kinematic characteristics, for example, the multi-input and multi-output for the ATV control system. Thus, it is challenging for the developed control methods to control the ATV in a complex maneuver accurately. To this end, our work provides a practical method for the path planning and path tracking of ATVs.
• The simulation of several path-tracking cases has demonstrated that the standard-MPC controller cannot accurately control the ATV to follow a path with varying curvature. However, the proposed AMPC controller outperforms the standard-MPC controller, while the AMPC controller can achieve the same level of tracking performance compared to the nonlinear MPC controller.

However, the proposed Hybrid A-star planning method has the drawback of extensive computation time, which could be improved by the refined algorithm structure in further research. Moreover, the proposed AMPC method is applied in the kinematic control of the ATV, which could not deal with the high-speed driving condition. In a further study, we will focus on the dynamic control of ATVs and apply the AMPC method in the dynamic control of ATVs.