Application of CILQR-Based Motion Planning and Tracking Control to Intelligent Tracked Vehicles

Abstract

To improve the safety of planned paths and the accuracy of tracking control for intelligent tracked vehicles, this paper investigates the application of a CILQR-based motion-planning and tracking-control framework to intelligent tracked vehicles. Firstly, based on an improved discrete-point quadratic smoothing algorithm and the adapted CILQR, collision-free multi-objective optimal path generation in dynamic environment is achieved. Secondly, based on the discretization error model of the intelligent tracked vehicle, an LQR-MPC hybrid control method is proposed based on switching strategy. Finally, an experimental platform is formed, and real-vehicle tests are carried out. Experimental results demonstrate the efficiency and accuracy of the proposed framework. The adapted CILQR algorithm significantly reduces computation time to approximately 1.5 ms per iteration, ensuring real-time performance. Furthermore, field tests confirm that the hierarchical LQR-MPC controller achieves robust tracking with an average lateral error of only 5.7 cm at a speed of 0.5 m/s, effectively validating the system’s capability in obstacle avoidance and precise trajectory tracking.

1. Introduction

Intelligent tracked vehicles, due to their superior ground adaptability and obstacle-surmounting capability, exhibit remarkable environmental adaptability under complex operating conditions [1]. By expanding the ground contact area through tracks, these vehicles achieve a uniform load distribution, which significantly reduces the risk of sinking and effectively suppresses slippage, thus ensuring stable operation on soft soils, steep slopes, and other extreme terrains. Moreover, intelligent tracked vehicles can be equipped with modular operational tools to support various functions, allowing wide applications in engineering construction, agricultural production, and mining operations [2].

In the field of motion planning, classical algorithms such as Dijkstra and A* search are capable of generating collision-free paths. However, their main limitation lies in the neglect of vehicle kinematic constraints, which requires additional smoothing processes on the coarse paths to satisfy practical application requirements [3,4]. Sampling-based methods are well suited for motion planning in complex environments but often produce solutions of unstable quality and struggle to directly address planning tasks involving complex motion constraints [5]. In contrast, interpolation-based approaches, such as spline curves [6] and Bézier curves [7], can generate smooth trajectories through curve fitting. Nevertheless, these methods often fail to adequately account for vehicle kinematic or dynamic constraints. In recent years, optimization-based approaches have emerged as the main methods for complex trajectory planning, as they allow simultaneous handling of multi-constraint and multi-objective problems [8,9,10]. Concurrently, other advanced strategies have also been proposed to enhance collision avoidance capabilities. For instance, Bao et al. introduced a Spatio-Temporal Graph Network (STGN) to achieve real-time and generalizable trajectory planning [11], while Bao et al. utilized Riemannian manifolds to develop a geodesic-based path planning method for transfer robots [12]. By establishing vehicle motion models, these approaches reformulate the motion planning task as a constrained optimal control problem, yielding smooth trajectories that satisfy both kinematic and dynamic constraints.

To achieve efficient and practical motion planning, a hierarchical strategy can be adopted. First, the A* algorithm is applied on a grid map to perform a global path search, obtaining an initial optimal path. Then, a discrete-point quadratic smoothing algorithm is utilized to refine the coarse path and enhance trajectory continuity. Finally, the constrained iterative linear quadratic regulator (CILQR) algorithm is used for dynamic real-time obstacle avoidance, further optimizing the trajectory to ensure that the generated path is not only collision-free but also compliant with the vehicle kinematic constraints.

Currently, vehicle trajectory tracking control methods include fuzzy control [13], sliding mode control [14], PID control [15], linear quadratic regulator (LQR) [16], and model predictive control (MPC) [17]. While advanced data-driven methods, such as neuroadaptive reinforcement learning show great potential in handling complex nonlinearities, they often impose high computational burdens and lack the interpretability required for safety-critical systems. In contrast, model-based methods like LQR and MPC remain the preferred choice for engineering applications due to their proven stability, real-time feasibility, and reliability in real-world deployment. Yan et al. [18] addressed the path-tracking-control problem of tracked mobile robots considering track-slip effects. They proposed a sliding-mode-control method that integrated adaptive laws with nonlinear control, effectively mitigating chattering and extending actuator lifespan. Hossain et al. [19] developed a hybrid feedforward–LQR control approach that significantly reduced lateral tracking errors in autonomous vehicles; experimental results demonstrated that this method significantly improved tracking accuracy in sharp-turn scenarios. Recent studies have also focused on enhancing control robustness against complex environmental factors. For instance, a secure observer-based collision-free control framework was developed for autonomous vehicles operating under non-Gaussian noises [20], and a cloud-based collision-avoidance adaptive cruise control strategy was proposed to handle external disturbances using token bucket shapers [21]. These methods illustrate the growing trend towards integrating advanced observation and communication technologies into vehicle control systems.

For intelligent tracked vehicles, the inherent characteristics of the nonlinear model and the strong coupling between longitudinal and lateral dynamics render single control methods insufficient to simultaneously ensure high-precision trajectory tracking and driving stability. As a result, hybrid control strategies have emerged as an effective solution to complex trajectory tracking problems. By integrating the strengths of multiple control approaches, such strategies allow dynamic adjustment of control logic according to system characteristics or varying operating conditions, thereby achieving higher tracking accuracy and enhanced system stability in complex environments.

To address these challenges, this paper presents the application of a hierarchical motion planning and tracking control method to intelligent tracked vehicles. Through collaborative algorithm optimization, the proposed framework ensures the planning of optimal collision-free paths in dynamic environments, effectively enhancing both the safety and tracking accuracy of intelligent tracked vehicles. The main contributions of this work are summarized as follows:

(1)

A hierarchical motion planning method is proposed. To address the limitations of existing motion planning approaches in terms of safety and feasibility, a KD-Tree-indexed grid network map is constructed. By integrating global path search with a dynamically constrained path smoothing algorithm, smooth and collision-free paths are generated in static environments. Furthermore, the application of the CILQR algorithm is successfully implemented on tracked platforms. By incorporating mechanism-specific constraints, such as curvature limitations, it addresses the adaptability issues of standard algorithms [22,23] on tracked platforms, thus improving both safety and feasibility in motion planning.

(2)

An LQR-MPC hybrid tracking control method is designed. To overcome the difficulty of a single controller in balancing rapid convergence and high-precision tracking, a switching-based LQR-MPC hybrid control strategy is proposed. Specifically, the LQR controller ensures fast convergence in large-error stages, while the MPC controller improves steady-state accuracy in small-error stages.

(3)

An experimental platform is developed, on the basis of which the proposed methods are validated. An experimental platform for intelligent tracked vehicles is constructed by integrating a tracked chassis, a high-precision positioning system, and a hierarchical modular software architecture, supporting the implementation of the proposed motion-planning and tracking-control methods in real vehicles. Field experiments, including autonomous obstacle-avoidance scenarios, demonstrate the safety, tracking accuracy, and engineering practicality of the proposed approach in real-world environments.

The remainder of this paper is organized as follows. Section 2 establishes the nonlinear kinematic model of the intelligent tracked vehicle and derives the discrete linear error model through linearization and discretization. Section 3 presents the hierarchical motion-planning method, including global path planning based on A*, path smoothing under dynamic obstacle-avoidance constraints, and local motion planning using the adapted CILQR algorithm. Section 4 designs the hybrid tracking-control method LQR-MPC, where a smooth switching strategy is developed and an LQR-MPC hybrid controller is constructed. Section 5 conducts validation experiments on both simulation and real-vehicle platforms to evaluate the safety and tracking accuracy of the proposed method. Finally, Section 6 concludes the study and discusses future research directions.

2. Kinematic Model of Intelligent Tracked Vehicle

In this section, a nonlinear kinematic model of the intelligent tracked vehicle is established. Based on this model, a discrete linear error kinematic model of the intelligent tracked vehicle is derived through linearization and discretization.

2.1. Nonlinear Kinematic Model of Intelligent Tracked Vehicle

Based on the differential-steering characteristics of practical tracked vehicles, a kinematic model is constructed, as illustrated in Figure 1.

In Figure 1, $X_g{O}_g{Y}_g$ denotes the global coordinate system, while $X_c{O}_c{Y}_c$ represents the body-fixed coordinate system attached to the center of mass of the vehicle. $O_c\left(x_c,y_c\right)$ indicates the coordinates of the center of mass of the vehicle. $v_l$ and $v_r$ denote the linear velocities of the left and right tracks, respectively. $v_c$ is the vehicle’s driving speed, defined as the average of the left and right track speeds, with its direction consistent with that of the track speeds. w represents the yaw rate, which is positive in the counterclockwise direction. ${\theta }_c$ is the angle of yaw of the center of mass of the vehicle. $B_d$ denotes the distance between the centers of the left and right tracks.

The nonlinear kinematic model of the intelligent tracked vehicle can be expressed as follows. Note that this ideal model is adopted as a valid approximation for the low-speed operating scenarios targeted in this work.

$$\left[\begin{array}{c}\dot{x}\\ \dot{y}\\ \dot{\theta }\end{array}\right]=\left[\begin{array}{c}{\upsilon }_{c}cos\theta \\ {\upsilon }_{c}sin\theta \\ \omega \end{array}\right]$$

(1)

where x and y denote the coordinates of the vehicle’s center of mass in the global coordinate system, $\theta$ represents the yaw angle of the center of mass of the intelligent tracked vehicle, and $v_c$ denotes the driving velocity of the intelligent tracked vehicle, defined as the average of the linear velocities of the left and right tracks, which can be expressed as

$${\upsilon }_c={\displaystyle \frac{{\upsilon }_l+{\upsilon }_r}{2}}$$

(2)

Let w represent the yaw rate of the intelligent tracked vehicle, which can be expressed as

$$w={\displaystyle \frac{{\upsilon }_r-{\upsilon }_l}{B_d}}$$

(3)

Currently, intelligent tracked vehicles commonly adopt a drive-by-wire chassis, with control input defined as track velocities ${[v_l,v_r]}^T$. By substituting (2) and (3) into (1), the state equation of the nonlinear kinematic model is obtained [16]:

$$\left[\begin{array}{c}\dot{x}\\ \dot{y}\\ \dot{\theta }\end{array}\right]=\left[\begin{array}{cc}{\displaystyle \frac{1}{2}}cos\theta & {\displaystyle \frac{1}{2}}cos\theta \\ {\displaystyle \frac{1}{2}}sin\theta & {\displaystyle \frac{1}{2}}sin\theta \\ -{\displaystyle \frac{1}{B_d}}& {\displaystyle \frac{1}{B_d}}\end{array}\right]\left[\begin{array}{c}{\upsilon }_l\\ {\upsilon }_r\end{array}\right]$$

(4)

2.2. Linear Error Model of Intelligent Tracked Vehicle

To enhance the implementability of the control strategy while meeting the requirements of the tracking control task for intelligent tracked vehicles, the model presented above is linearized around the reference point using a Taylor series expansion, thereby transforming (4) into a linear error kinematic model. In detail, the state vector is defined as $X={[x,y,\theta ]}^T$, and the control input is defined as $U={[v_l,v_r]}^T$.

For the reference trajectory, at each time step, the vehicle state must be associated with the closest reference state on the trajectory:

$$\dot{X}=f\left(X,U\right)\to {\dot{X}}_r=f\left(X_r,U_r\right)$$

(5)

By performing a first-order Taylor expansion of Equation (5) around the reference point, the vehicle’s kinematic model is converted into an error kinematic model:

$$\dot{X}-{\dot{X}}_r={\displaystyle \frac{\partial f\left(X,U\right)}{\partial X}}{\mid }_{X_r,U_r}\left(X-X_r\right)+{\displaystyle \frac{\partial f\left(X,U\right)}{\partial U}}{\mid }_{X_r,U_r}\left(U-U_r\right)$$

(6)

The state vector at this time, represented by $X_e=X-X_r$, $U_e=U-U_r$, is

$$\begin{array}{cc}\hfill {\dot{X}}_e& =\left[\begin{array}{ccc}0& 0& -{\displaystyle \frac{\left({\upsilon }_{lr}+{\upsilon }_{rr}\right)}{2}}sin{\theta }_r\\ 0& 0& {\displaystyle \frac{\left({\upsilon }_{lr}+{\upsilon }_{rr}\right)}{2}}cos{\theta }_r\\ 0& 0& 0\end{array}\right]X_e+\left[\begin{array}{cc}{\displaystyle \frac{1}{2}}cos{\theta }_r& {\displaystyle \frac{1}{2}}cos{\theta }_r\\ {\displaystyle \frac{1}{2}}sin{\theta }_r& {\displaystyle \frac{1}{2}}sin{\theta }_r\\ -{\displaystyle \frac{1}{B_d}}& {\displaystyle \frac{1}{B_d}}\end{array}\right]\left[\begin{array}{c}{\upsilon }_l-{\upsilon }_{lr}\\ {\upsilon }_r-{\upsilon }_{rr}\end{array}\right]\hfill \\ \hfill & =A{X}_e+B{U}_e\hfill \end{array}$$

(7)

The continuous linear error kinematic model is discretized so that the derived model can be used directly by the controller. The derivation of this linear error model relies on the small-error assumption, which implies that the vehicle’s state does not deviate significantly from the reference trajectory. Although tracked vehicles involve complex nonlinear dynamics and track–ground interactions, this linearization approach is widely adopted in tracking-control literature [16,17] and is considered effective for feedback control systems. Based on Equation (7), by using the mean value theorem for integrals and applying the midpoint Euler method, we get

$$X_e\left(t+1\right)={\left(I-{\displaystyle \frac{1}{2}}AT\right)}^{-1}\left(I+{\displaystyle \frac{1}{2}}AT\right)X_e\left(t\right)+BT{U}_e\left(t\right)$$

(8)

where T denotes a constant representing the sampling interval and I is the identity matrix.

3. Motion-Planning Design

To achieve motion planning for intelligent tracked vehicles, a KD-Tree-indexed grid network map is constructed, and the A* algorithm based on Chebyshev distance is used to perform the optimal global path search, thereby generating an initial collision-free path. Taking into account the geometric configuration characteristics of intelligent tracked vehicles, dynamic obstacle-avoidance constraints are designed, and a discrete-point quadratic smoothing algorithm is improved based on these constraints to optimize the smoothness and continuity of the coarse path. Meanwhile, for local motion planning, a motion model of intelligent tracked vehicles is established and the CILQR algorithm is improved to adapt it to optimize the dynamic trajectory of intelligent tracked vehicles. By integrating the above methods, a hierarchical motion-planning framework is proposed, which combines global path search, trajectory smoothing, and local dynamic optimization. This framework enables the execution of motion-planning tasks from the start point to the target point on an environmental map that incorporates both static prior information and dynamic real-time information.

3.1. Grid Network Map and Global Path Planning

To ensure real-time performance and accuracy in motion planning for tracked vehicles, this paper constructs a grid network map optimized with KD-Tree spatial indexing. Although traditional grid maps face a trade-off between resolution and computational efficiency, the integration of KD-Tree reduces the nearest-neighbor search complexity from $O\left(n\right)$ to $O(logn)$, enabling efficient obstacle representation without additional topological layers. Based on this indexed map, the A* algorithm is employed for global path planning. Using a cost function $F\left(n\right)=G\left(n\right)+H\left(n\right)$ with Chebyshev distance as heuristic, the algorithm efficiently generates a collision-free global optimal trajectory, providing a foundation for subsequent trajectory optimization and tracking control.

3.2. Reference Path Smoothing Considering Dynamic Obstacle-Avoidance Constraints

Within the hierarchical motion-planning framework proposed in this section, the A* algorithm based on the Chebyshev distance is used for global path planning, aiming to minimize time consumption and path length, thereby generating a globally optimal collision-free path. However, in an eight-connected grid network map, the path generated by the A* algorithm exhibits geometric defects such as 45° and 90° corner turns, which lead to abrupt yaw angle changes of up to 90° and discontinuous points in curvature where the variation reaches 3.14 rad/m². Experimental results show that when a tracked vehicle follows such a path at a speed of 0.5 m/s, track slippage and turning instability frequently occur, significantly compromising driving stability. Therefore, a reference path-smoothing algorithm is required to optimize the path generated by the A* algorithm.

The reference path-smoothing task is designed with the following four core performance objectives:

(1)

Smoothness: Ensure curvature continuity, reduce peak curvature, minimize fluctuations in lateral acceleration, and improve vehicle steering dynamic stability.

(2)

Similarity: Maintain spatial consistency between the smoothed path and the original path, preserve obstacle-avoidance decisions, and retain global optimality.

(3)

Compactness: Ensure uniform distribution of path points while controlling the increase in path length.

(4)

Safety: Guaranty that the minimum distance between the smoothed path and obstacles is greater than the radius of the geometric envelope of the vehicle, thus satisfying the obstacle-avoidance requirements.

Based on the above objectives, a discrete-point quadratic smoothing algorithm is designed. By constructing a multi-objective cost function that incorporates smoothness, similarity, and compactness, and introduces adaptive safety constraints, the proposed method achieves a coordinated optimization of path quality and safety.

Figure 2 illustrates the reference path-smoothing algorithm. The original path point coordinates are denoted as $P_i(x_i,y_i)$, while the smoothed path point coordinates are denoted as $P_i^{\prime }(x_i^{\prime },y_i^{\prime })$. The smoothing process is achieved by shifting the discrete points along the original reference path, where A represents the distance segment between the original path points and B denotes the corresponding offset after the smoothing adjustment.

Assume that the global path in the Cartesian coordinate system is represented by the point set:

$$P=\{p_1,p_2,\cdots ,p_n\},p_i=(x_i,y_i)$$

(9)

The smoothed path point set is defined as

$$Q=\{q_1,q_2,\cdots ,q_n\},q_i=({\widehat{x}}_i,{\widehat{y}}_i)$$

(10)

To satisfy the requirement of path smoothness, a linear smoothness cost is established. The smoothness of the path is measured by the angle formed by three adjacent points $q_{i-1},q_i,q_{i+1}$. A larger angle indicates a smoother path, and the cost function is given by

$$Cos{t}_{smooth}={\displaystyle \sum _{i=2}^{n-1}}{\parallel q_i-2q_{i-1}+q_{i+1}\parallel }^2$$

(11)

To meet the requirement of path similarity, a similarity cost is defined. The similarity is measured by the squared offset distance between the smoothed path points and the original path points, expressed as

$$Cos{t}_{similar}={\displaystyle \sum _{i=1}^n}{\parallel q_i-p_i\parallel }^2$$

(12)

To meet the requirement of path compactness, a compactness cost is established. It is measured by the squared distance between adjacent smoothed path points, ensuring a uniform point distribution and a low increase rate in path length.

$$Cos{t}_{compact}={\displaystyle \sum _{i=1}^{n-1}}{\parallel q_{i+1}-q_i\parallel }^2$$

(13)

By combining the above objectives, the overall cost function for the reference path smoothing algorithm is formulated as

$$Cost=w_{1}Cos{t}_{smooth}+w_{2}Cos{t}_{similar}+w_{3}Cos{t}_{compact}$$

(14)

where $w_1,w_2,w_3$ are the weighting coefficients for smoothness, similarity, and compactness, respectively.

To ensure path safety, a safety constraint is introduced. Taking into account the differential-steering characteristics of tracked vehicles, the geometric envelope is approximated as a square with side length L, and the equivalent envelope radius is given by $r=L/\sqrt{2}$. Based on KD-Tree indexing, the distance between each path point and the nearest obstacle is calculated, and an adaptive dynamic offset constraint is designed as

$$\parallel q_i-p_i\parallel \le min(d_{\max },d_{\mathrm{obs}}\left(q_i\right)-r)$$

(15)

where $d_{\max }$ is the fixed upper limit of the offset, and $d\mathrm{obs}\left(q_i\right)$ denotes the distance from the point $q_i$ to the nearest obstacle.

Based on the aforementioned analysis, the reference path smoothing problem is transformed into a constrained multi-objective optimization problem. To align with the standard quadratic programming (QP) solver, we define the decision variable as a concatenated state vector $z={[{\widehat{x}}_1,{\widehat{y}}_1,{\widehat{x}}_2,{\widehat{y}}_2,\dots ,{\widehat{x}}_n,{\widehat{y}}_n]}^T$, which contains the coordinates of all points in the set Q. By converting the cost functions into quadratic forms and linearizing the safety constraints, the smoothing problem is formulated as follows:

$$\begin{array}{c}{min}_z{\displaystyle \frac{1}{2}}{z}^T(w_1{H}_s+w_2{H}_r+w_3{H}_c)z+f^{T}z\\ s.t.Az\le b\end{array}$$

(16)

where $H_s$, $H_r$ and $H_c$ denote the Hessian matrices corresponding to the smoothness, similarity, and compactness costs, respectively. f is the linear term coefficient vector derived from the original coordinates in set P. A and b define the linear inequality constraints based on the safety boundary.

The proposed reference path smoothing framework consists mainly of the traditional discrete-point quadratic smoothing algorithm and the improved discrete-point quadratic smoothing algorithm, with an online switching mechanism between the two. The main difference lies in the construction of the safety constraints. The traditional algorithm employs a fixed offset constraint $\parallel q_i-p_i\parallel \le d_{max}$, which is suitable for environments without obstacles or low safety requirements. In contrast, the adapted algorithm incorporates KD-Tree indexing to dynamically compute distances to obstacles and adjust the offset upper bound (as in Equation (16)), thereby ensuring path safety. This makes it more applicable to unstructured road environments.

3.3. Local Motion Planning Based on Adapted CILQR

Under the condition that complete environmental information is available, the hierarchical motion-planning framework proposed in this chapter employs the A* algorithm based on the Chebyshev distance and a discrete-point quadratic smoothing algorithm to generate a collision-free and smooth global reference path. However, this approach relies on prior maps and is limited in handling dynamic environmental changes, such as construction area adjustments or interference from moving equipment.To address this issue, this section improves the CILQR algorithm proposed by Chen [22,23]. Unlike the standard CILQR frameworks, which are typically designed for Ackerman-steering vehicles with ideal kinematic constraints, the proposed improvement is specifically tailored to tracked vehicles. The proposed method introduces curvature as a state variable and incorporates a domain-specific cost function to explicitly address wear that is often neglected in previous CILQR implementations. Based on this, a local motion-planning algorithm is developed, which is capable of real-time path re-planning at the millisecond level, suitable for obstacle-avoidance requirements of tracked vehicles in dynamic environments. The improved algorithm establishes a nonlinear motion model, designs a cost function that integrates path smoothness and similarity, and employs KD-Tree indexing to construct efficient obstacle-avoidance constraints. The local motion-planning problem is then formulated as a constrained optimal control problem for a nonlinear system, which is transformed into an unconstrained problem via the penalty function method to accelerate computation. Experimental results show that the proposed method achieves a planning time of less than 400 ms per iteration, thereby satisfying the real-time requirements of tracked vehicle operations.

3.3.1. Formulation of the Local Path-Planning Problem

The local motion-planning problem of a tracked vehicle can be expressed in a general optimization form as

$$\begin{array}{c}{x}^{*},u^{*}=\mathrm{argmin}\left\{{\displaystyle \Phi \left(x_N\right)+{\displaystyle \sum _{k=0}^{N-1}}L(x_k,u_k)}\right\}\\ \mathrm{s}.\mathrm{t}.:x_{k+1}=f(x_k,u_k),k=0,1,\cdots ,N-1\\ x_0=x_{\mathrm{init}}\\ g(x_k,u_k)\le 0\\ h(x_k,u_k)=0\end{array}$$

(17)

where $x_k\in {\mathbb{R}}^n$ is the state vector, $u_k\in {\mathbb{R}}^m$ is the control input vector, and N is the prediction horizon. $\Phi \left(x_N\right)$ is the terminal cost and $L(x_k,u_k)$ is the stage cost. $f(x_k,u_k)$ represents the dynamics of the state transition. $g(·)\le 0$ and $h(·)=0$ represent the general inequality and equality constraints, respectively.

To satisfy real-time requirements, the following assumptions are introduced.

(1)

The state transition equation is the only equality constraint.

(2)

General inequality constraints are only distributed over state and control inputs across all time steps.

(3)

All cost and constraint functions possess continuous first- and second-order derivatives.

Under these assumptions, the general problem is simplified into a standard discrete finite-horizon motion-planning problem:

$$\begin{array}{c}{x}^{*},u^{*}=\mathrm{argmin}\left\{{\displaystyle \Phi \left(x_N\right)+{\displaystyle \sum _{k=0}^{N-1}}L(x_k,u_k)}\right\}\\ \mathrm{s}.\mathrm{t}.:x_{k+1}=f(x_k,u_k),k=0,1,\cdots ,N-1\\ x_0=x_{\mathrm{init}}\\ g_x\left(x_k\right)\le 0,g_u\left(u_k\right)\le 0,k=0,1,\cdots ,N-1\\ g_N\left(x_N\right)=0\end{array}$$

(18)

By imposing constraints on curvature, curvature variation, and obstacle avoidance, the local motion-planning problem of the tracked vehicle is formulated as

$$\begin{array}{c}\mathrm{argmin}J=\left({\displaystyle \frac{1}{2}}{x}_N^T{Q}_N{x}_N+q_N^T{x}_N+c_N\right)\\ +{\displaystyle {\sum }_{k=0}^{N-1}}\left({\displaystyle \frac{1}{2}}{x}_k^T{Q}_k{x}_k+q_k^T{x}_k+c_k+{\displaystyle \frac{1}{2}}{u}_k^T{R}_k{u}_k+r_k^T{u}_k\right)\\ \mathrm{s}.\mathrm{t}.:x_{k+1}=f(x_k,u_k),k=0,1,\cdots ,N-1\\ x_0=x_{\mathrm{init}}\\ x_k\in {\mathcal{X}}_{\mathrm{free}}=\mathcal{X}\setminus {\mathcal{O}}_k\end{array}$$

(19)

where $\mathcal{X}$ denotes the entire Cartesian space and ${\mathcal{O}}_k$ represents the space occupied by obstacles, $Q_i$ and $R_i$ are weighting matrices, and $q_i,r_i,c_i$ are linear and constant terms, respectively.

To handle nonlinear inequality constraints $g_x\left(x_k\right)\le 0$, a first-order Taylor expansion is applied for linearization:

$$g_x(x_k+\delta x_k)\approx g_x\left(x_k\right)+\nabla g_x{\left(x_k\right)}^T\delta x_k$$

(20)

The linearized inequality constraint can be incorporated into the cost function using a penalty function approach. The exponential penalty function is defined as

$$b_x^k=q_{1}exp\left(q_2{g}_x\left(x_k\right)\right),q_1,q_2>0$$

(21)

where $q_1$ and $q_2$ are parameters that regulate the shape of the penalty function.

Through this penalty method, the local motion-planning problem of the tracked vehicle is transformed into an unconstrained optimization problem:

$$\begin{array}{c}{x}^{*},u^{*}=\mathrm{argmin}\left\{{\displaystyle \Phi \left(x_N\right)+{\displaystyle \sum _{k=0}^{N-1}}L(x_k,u_k)+\Psi (x_k,u_k)}\right\}\\ \mathrm{s}.\mathrm{t}.:x_{k+1}=f(x_k,u_k),k=0,1,\cdots ,N-1\\ x_0=x_{\mathrm{init}}\end{array}$$

(22)

where $\Psi (x_k,u_k)$ represents the total penalty term (including $b_x^k$ and other penalties).

This unconstrained motion-planning problem can be efficiently solved by designing an ILQR-based approach, thereby ensuring real-time motion-planning performance.

3.3.2. Design of the Adapted CILQR Algorithm

To realize local motion planning based on the original reference path, the discrete model of the tracked vehicle is formulated as follows:

$$\left[\begin{array}{c}{x}_{i+1}\\ y_{i+1}\\ {\theta }_{i+1}\\ {\kappa }_{i+1}\end{array}\right]=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]\left[\begin{array}{c}{x}_i\\ y_i\\ {\theta }_i\\ {\kappa }_i\end{array}\right]+\left[\begin{array}{c}{v}_{i}cos{\theta }_{i}dt\\ v_{i}sin{\theta }_{i}dt\\ 0\\ 0\end{array}\right]+\left[\begin{array}{c}0\\ 0\\ 0\\ v_{i}dt\end{array}\right]{\dot{\kappa }}_l$$

(23)

where the state vector is defined as $X_i=[x_i,y_i,{\theta }_i,{\kappa }_i]$, representing the Cartesian coordinates, the yaw angle, and the curvature. The control input is $U={\dot{\kappa }}_l$, which denotes the curvature rate. $v_{i}dt$ represents the path length between time steps, which can be substituted by the Euclidean distance between discrete points $d{s}_i$.

For the local motion-planning task, the model is linearized around the reference point via a Taylor expansion, eliminating nonlinear terms in the matrix. The linearized model is expressed as

$$\left[\begin{array}{c}{x}_{i+1}\\ y_{i+1}\\ {\theta }_{i+1}\\ {\kappa }_{i+1}\end{array}\right]\approx \left[\begin{array}{cccc}1& 0& -v_i^{r}sin{\theta }_i^{r}dt& 0\\ 0& 1& v_i^{r}cos{\theta }_i^{r}dt& 0\\ 0& 0& 1& dt\\ 0& 0& 0& 1\end{array}\right]\left[\begin{array}{c}{x}_i\\ y_i\\ {\theta }_i\\ {\kappa }_i\end{array}\right]+\left[\begin{array}{c}0\\ 0\\ 0\\ v_i^{r}dt\end{array}\right]{\dot{\kappa }}_l$$

(24)

The cost function comprehensively considers the path deviation, curvature, and curvature rate:

$$J=J^{\mathrm{ref}}+J^{\kappa }+J^{\dot{\kappa }}$$

(25)

(1)

Path deviation cost $J^{\mathrm{ref}}$:

$$J^{\mathrm{ref}}={\displaystyle \sum _{i=1}^{N-1}}{(X_i-X_i^{\mathrm{ref}})}^T{Q}_i^{\mathrm{ref}}(X_i-X_i^{\mathrm{ref}})+{(X_N-X_{\mathrm{goal}})}^T{Q}_N^{\mathrm{ref}}(X_N-X_{\mathrm{goal}})$$

(26)

where $X_i^{\mathrm{ref}}$ denotes the reference path point, $X_{\mathrm{goal}}$ the target point, and $Q_i^{\mathrm{ref}},Q_N^{\mathrm{ref}}$ the weighting matrices. A large terminal weighting $Q_N^{\mathrm{ref}}\gg Q_i^{\mathrm{ref}}$ ensures convergence toward the target neighborhood, reducing iterations and improving global path-following accuracy.

(2)

Curvature cost $J^{\kappa }$:

$$J^{\kappa }=w_{\kappa }{\displaystyle \sum _{i=0}^N}{X}_i^T\left[\begin{array}{cccc}0& 0& 0& 0\\ 0& 0& 0& 0\\ 0& 0& 0& 0\\ 0& 0& 0& 1\end{array}\right]X_i$$

(27)

where $w_{\kappa }$ is the weight of curvature. Curvature ${\kappa }_i$ determines the minimum turning radius and influences track wear and dynamic stability through lateral acceleration $a_{lat}=v^2\kappa$.

(3)

Curvature rate cost $J^{{\kappa }^{\prime }}$:

$$J^{{\kappa }^{\prime }}={\displaystyle \sum _{i=0}^{N-1}}{U}_i^T{w}_{{\kappa }^{\prime }}{U}_i$$

(28)

where $w_{{\kappa }^{\prime }}$ is the weight of the curvature rate. The curvature rate ${\kappa }^{\prime }={\displaystyle \frac{d\kappa }{ds}}$ denotes the spatial derivative of curvature with respect to the arc length, reflecting the changing rate of the path’s bending geometry. An excessive curvature rate aggravates the sliding friction of the tracks. Optimizing ${\kappa }^{\prime }$ improves tracking accuracy and ride comfort.

Inequality constraints are set as follows.

(1)

Curvature bound constraint:

$${\kappa }_{\mathrm{low}}\le X_i^T\left[\begin{array}{cccc}0& 0& 0& 0\\ 0& 0& 0& 0\\ 0& 0& 0& 0\\ 0& 0& 0& 1\end{array}\right]X_i\le {\kappa }_{\mathrm{high}},{\kappa }_{\mathrm{low}}=-{\kappa }_{\mathrm{high}}$$

(29)

(2)

Curvature rate bound constraint:

$${\kappa }_{\mathrm{low}}^{\prime }\le U_i\le {\kappa }_{\mathrm{high}}^{\prime },{\kappa }_{\mathrm{low}}^{\prime }=-{\kappa }_{\mathrm{high}}^{\prime }$$

(30)

To ensure consistency between curvature and curvature rate constraints, the following relationship is imposed:

$$\begin{array}{c}{\kappa }_{i+1}={\kappa }_i+{\kappa }_i^{\prime }ds,{\kappa }_i\in \left[{\kappa }_{\mathrm{low}},{\kappa }_{\mathrm{high}}\right]\\ {\kappa }^{\prime }\le {\displaystyle \frac{\left|{\kappa }_{\mathrm{high}}-{\kappa }_{\mathrm{low}}\right|}{ds}}\end{array}$$

(31)

(3)

Hybrid obstacle-avoidance constraints:

Based on the grid map and the KD-Tree nearest-neighbor search, a hybrid obstacle-representation framework is proposed combining the grid map and the convex polygonal envelope (Figure 3).

For dynamic obstacles in uncertain environments, the occupancy space is represented using a grid-based model (Figure 3, left). The KD-Tree indexing stores grid nodes to construct avoidance constraints. If the optimized path approaches an obstacle below the safety threshold, a convex polygonal envelope (Figure 3, right) is employed for precise collision detection.

Taking into account the differential-steering characteristics of the tracked vehicles, a circular envelope is adopted for vehicle representation (Figure 4).

The parametric form is

$$V\left(q\right)=\{x\in {\mathbb{R}}^2|\parallel x-q_c\parallel \le r\},r={\displaystyle \frac{L}{2}}+s^{\mathrm{buffer}}$$

(32)

where $q_c$ is the coordinate of the center, r is the radius of the envelope, L is the length of the vehicle and $s^{buffer}$ is the boundary buffer.

The obstacle-avoidance constraint is expressed as Figure 5.

The distance function $d\left(q\right)$ is defined as the minimum distance between the vehicle envelope and the obstacle:

$$d\left(q\right)=\underset{o_i\in {\mathbb{O}}_p}{min}\parallel q_c-o_i\parallel -r\ge 0$$

(33)

The solution process of the CILQR algorithm is illustrated in Algorithm 1.

Algorithm 1 The CILQR Algorithm

1: Initialization: Provide an initial state trajectory sequence X and a control sequence U, with parameters $q_1>1,q_2>1,0<\alpha <1$;   2: while Constraints not satisfied (Outer Loop) do   3:    Step 1: Update Penalty Function   4:    Transform the inequality constraints into the cost function using exponential penalty: $b_x^k=q_{1}exp\left(q_2{p}^k\left(x_k\right)\right)$;   5:    while Optimization not converged (Inner Loop) do   6:      Step 2: Backward Pass   7:      Linearize the system and compute the control update $\overline{U}$ via backward propagation;   8:      while Line search condition not met do   9:         Step 3: Line Search 10:         Update control input $\overline{U}\leftarrow \alpha \overline{U}$; 11:         Check regularity condition $z\in [1e^{-4},10]$; 12:      end while 13:      Step 4: Forward Pass 14:      Perform forward propagation using $\overline{U}$ to compute the updated trajectory $X^{*}$; 15:    end while 16:    Update global trajectory $X=X^{*}$ and control $U=\overline{U}$; 17: end while 18: End when global optimization converges.

4. Trajectory Tracking Control Design

The trajectory tracking task of an intelligent tracked vehicle aims to regulate the linear velocities of the left and right tracks such that the vehicle can stably follow a given reference trajectory from any initial position and orientation within a finite time horizon, ultimately reaching the target state. In complex operational scenarios such as emergency rescue and high-precision agriculture, higher demands are placed on tracking accuracy and stability, making it difficult for a single controller to satisfy performance requirements under varying conditions. To address this challenge, a hybrid LQR-MPC control method is designed on the basis of the derived discrete linear error kinematic model. Using a smooth switching strategy, this method integrates the stability of the LQR with the predictive optimization capability of MPC, thus significantly enhancing the reference trajectory tracking accuracy and ensuring adaptability to diverse operating conditions of differential-steering tracked vehicles.

4.1. Design of the LQR-MPC Hybrid Controller

Both LQR and MPC controllers are designed based on the discrete-time linearized error model:

$$x_{k+1}=A{x}_k+B{u}_k$$

(34)

where $x={[e_x,e_y,e_{\psi }]}^T$ represents the tracking errors and $u={[\Delta v_L,\Delta v_R]}^T$ represents the velocity control increments. It is noted that the planning state $X_i={[x_i,y_i,{\theta }_i,{\kappa }_i]}^T$ derived in Section 3.3.2 is decoupled for the tracking controller: the position and heading components $(x_i,y_i,{\theta }_i)$ define the reference trajectory for computing the error state x, while the curvature ${\kappa }_i$ is utilized to compute the reference yaw rate and update the linearized system matrices for model predictive control.

(1)

LQR Controller Design:

The LQR controller is designed to achieve optimal regulation over an infinite horizon. The objective is to minimize the following quadratic cost function:

$$J={\displaystyle \sum _{k=0}^{\infty }}\left(x_k^T{Q}_L{x}_k+u_k^T{R}_L{u}_k\right)$$

(35)

where $Q_L=\mathrm{diag}(q_1,q_2,q_3)$ and $R_L=\mathrm{diag}(r_1,r_2)$ are the state and control weighting matrices. The optimal control law is expressed as $U_{\mathrm{LQR}}=-K{x}_k$. The gain matrix K is derived from

$$K={(R_L+B^{T}PB)}^{-1}{B}^{T}PA$$

(36)

where P is the steady-state solution to the discrete-time algebraic Riccati equation (DARE):

$$P=A^{T}PA-\left(A^{T}PB\right){(R_L+B^{T}PB)}^{-1}\left(B^{T}PA\right)+Q_L$$

(37)

LQR provides a computationally efficient solution for rapid error convergence, though it does not inherently handle physical constraints.

(2)

MPC Controller Design:

The MPC framework adopts a finite-horizon receding optimization strategy, in which system constraints are explicitly incorporated to ensure safe and feasible control actions.

Let $N_p$ and $N_c$ denote the prediction horizon and control horizon, respectively. Taking the current time step k as the reference, the predicted future states and control sequences are concatenated into the following vectors:

$$X_e\left(k\right)=\left[\begin{array}{c}{x}_{k+1|k}\\ x_{k+2|k}\\ ⋮\\ x_{k+N_p|k}\end{array}\right],U_e\left(k\right)=\left[\begin{array}{c}{u}_{k|k}\\ u_{k+1|k}\\ ⋮\\ u_{k+N_c-1|k}\end{array}\right].$$

(38)

By recursively applying the system model in Equation (34), the predicted states along the prediction horizon can be written in compact form as

$$X_e\left(k\right)=\mathcal{A}{x}_k+\mathcal{B}{U}_e\left(k\right),$$

(39)

where $\mathcal{A}$ and $\mathcal{B}$ are the prediction matrices constructed from the system matrices A and B. These matrices describe the evolution of the error states under a sequence of future control actions.

The MPC cost function penalizes deviations between the predicted and reference trajectories, while simultaneously regularizing control efforts. The optimization problem is formulated as

$$\begin{array}{c}{\displaystyle \underset{U_e\left(k\right)}{min}{\displaystyle \sum _{i=1}^{N_p}}\frac{1}{2}{X}_e{(k+i|k)}^{T}Q{X}_e(k+i|k)+{\displaystyle \sum _{j=0}^{N_c-1}}{U}_e{(k+j|k)}^{T}R{U}_e(k+j|k)}\\ \mathrm{s}.\mathrm{t}.:x_{k+i+1|k}=A{x}_{k+i|k}+B{u}_{k+i|k},i=0,1,\dots ,N_p-1\\ U_{min}\le U_e(k+j|k)\le U_{max},j=0,1,\dots ,N_c-1\end{array}$$

(40)

Here, Q and R denote positive semi-definite state and input weighting matrices, respectively. The constraints $U_{min}$ and $U_{max}$ incorporate both actuator physical limits and the allowable bounds for the reference velocity, thereby preventing infeasible or unsafe commands. It is important to note that to guarantee the feasibility of the optimization problem and avoid potential solver failures during mode switching, hard constraints are applied only to the control inputs ($U_{min},U_{max}$) based on physical actuator limits. State constraints are handled as soft penalties through the quadratic cost function, ensuring that a feasible solution always exists.

Solving the optimization problem yields the optimal control sequence $\left\{U_e^{*}\right\}$. According to the receding horizon principle, only the first control action $U_e^{*}\left(0\right)$ is applied to the tracked vehicle. The actual applied command is obtained by combining the optimal control error with the reference track velocities:

$$U_{\mathrm{MPC}}=U_r\left(0\right)+U_e^{*}\left(0\right).$$

(41)

At each sampling step, the reference state $X_{\mathrm{ref}}$ and reference control input $U_{\mathrm{ref}}$ are dynamically updated. This is achieved by locating the nearest point on the reference trajectory to the current vehicle pose, ensuring that the MPC optimization remains locally consistent with the desired path. By repeating this procedure in a receding manner, the controller continuously compensates for model errors and disturbances, enabling robust and accurate trajectory tracking for the intelligent tracked vehicle.

4.2. Control Framework

To integrate the high-speed convergence of LQR with the constraint-handling precision of MPC, a smooth switching strategy is employed. The integrated control framework selects the appropriate control mode based on the weighted state error magnitude.The hybrid control input $U_{\mathrm{hybrid}}$ is defined as

$$U_{\mathrm{hybrid}}=\left\{\begin{array}{cc}{U}_{\mathrm{LQR}},\hfill & {\left|X_e\right|}_Q>\vartheta \hfill \\ U_{\mathrm{MPC}},\hfill & {\left|X_e\right|}_Q\le \vartheta \hfill \end{array}\right.$$

(42)

where ${\left|X_e\right|}_Q=\sqrt{{X_e}^{T}Q{X}_e}$ represents the weighted error norm, $Q=\mathrm{diag}(q_{x_e},q_{y_e},q_{{\theta }_e})$ is the error weighting matrix and $\vartheta$ is the predefined switching threshold. As illustrated in Figure 6, the framework operates by prioritizing the LQR controller during the large-error phase to rapidly pull the system state toward the reference trajectory while minimizing computational overhead. Once the weighted error falls below the threshold $\vartheta$, the system seamlessly switches to the MPC mode for fine-grained tracking and rigorous constraint satisfaction. To ensure a smooth transition between $u_{\mathrm{LQR}}$ and $u_{\mathrm{MPC}}$ and to prevent chattering, the control input increment is strictly limited based on the maximum acceleration capabilities of the drive motors. Mathematically, this is enforced as $|u_k-u_{k-1}|\le \Delta u_{max}$. This constraint guaranties that the control action remains continuous and physically feasible, effectively filtering out high-frequency oscillations that might arise from the threshold-based switching rule.

5. Simulation and Experimental Validation

5.1. Planning Simulation Verification

To validate the proposed hierarchical motion-planning framework for intelligent tracked vehicles, a high-fidelity numerical simulation platform was established using the robot operating system (ROS). The experiments primarily evaluated the robustness of the method in dynamic environments, with specific focus on handling static variations and dynamic obstacles. Note that the term “CILQR” refers to the adapted algorithm throughout this study. The experimental setup employed a Lenovo Legion R9000P high-performance laptop (Lenovo, Beijing, China) as the upper-level planner, configured with Ubuntu 20.04 LTS and ROS Noetic (Noetic Ninjemys).

Within the simulation platform, the A* algorithm was used to generate the global path as a reference baseline, while the improved discrete-point quadratic smoothing algorithm was applied to generate the initial solution. Then, dynamic obstacle avoidance was performed using the pose of the real-time vehicle as the starting point. The vehicle dimension parameters were scaled down by a ratio of 2:1 relative to the experimental platform. The parameter settings for the A* algorithm and the smoothing algorithm are listed in

Table 1. Simulation parameters of A* algorithm and smoothing algorithm.

Parameter	Unit	Value
$p_{start}$	-	(2, 2)
$p_{goal}$	-	(40, 40)
Grid Size	m	0.25
R	m	2.0
$b_{max}$	m	1
$q_1$	-	5
$q_2$	-	2
$q_3$	-	2

, and the detailed parameters of the adapted CILQR algorithm are provided in

Table 2. Simulation parameters of the adapted CILQR algorithm.

Parameter	Unit	Value
$w_x$	-	2.5
$w_y$	-	2.5
$w_{\theta }$	-	0.0
$w_{\kappa }$	-	100
$w_{\dot{\kappa }}$	-	2500
$w_x^N$	-	25
$w_y^N$	-	25
$w_{\theta }^N$	-	100
$q_1^{\kappa }$	-	3
$q_2^{\kappa }$	-	20
$q_1^{\dot{\kappa }}$	-	3
$q_2^{\dot{\kappa }}$	-	20
$q_1^{Obs}$	-	6
$q_2^{Obs}$	-	4
${\kappa }_{low}$	-	−0.20
${\kappa }_{high}$	-	0.20
${\dot{\kappa }}_{low\_begin}$	-	−1.50
${\dot{\kappa }}_{high\_begin}$	-	1.50
$K_{safe}$	m	1.2
$R_{ego}$	m	1.0
$L_{ego}$	m	1.6
$W_{ego}$	m	1.2
${\gamma }_1$	-	$1\times {10}^{-4}$
${\gamma }_2$	-	100
$\alpha$	-	1.0
$\beta$	-	0.5

.

Two dynamic simulation scenarios were designed:

(1)

Simulation Environment 1: A relatively open area containing temporary construction obstacles that cause interference between the global reference path and static obstacles.

(2)

Simulation Environment 2: A confined area populated with other working vehicles, resulting in interference between the global reference path and dynamic obstacles.

To comprehensively evaluate the performance of the algorithm, three operating conditions were set for each environment by selecting different starting points on the reference path (the 0th, 30th and 60th coordinate points) as the CILQR planning initiation points, denoted as “CILQR-0”, “CILQR-30” and “CILQR-60”. In all experimental scenarios, the perception system updates the environment in real time and detects intersections between the path and obstacles.

Simulation Experiment 1: As shown in Figure 7, the local motion-planning results for temporary construction obstacles in the relatively open environment are presented. Figure 7a corresponds to the “condition CILQR-30”and Figure 7b corresponds to the “condition CILQR-60”. In the figures, black polygons denote obstacles, black polygons overlaid on the blue dashed line denote temporary construction obstacles, the yellow solid line denotes the original global path, the blue dashed line denotes the smoothed reference path, and the red trajectory denotes the optimized result produced by the adapted CILQR. The results indicate that the adapted CILQR effectively avoids obstacles, generates smooth and safe driving trajectories, and maintains consistency with the global reference path.

Figure 8 presents the detailed data analysis of Experiment 1. Figure 8a shows the curvature comparison, where the curvature variation in the adapted CILQR trajectory is continuous and smooth, and its trend is consistent with that of the reference path, indicating that the algorithm effectively exploits global path information. Figure 8b illustrates the curvature rate comparison, in which the adapted CILQR demonstrates smaller fluctuations in the curvature rate, thus improving the driving stability of the tracked vehicle. Figure 8d depicts the minimum distance comparison to obstacles, where the adapted CILQR consistently maintains a safe distance under all three conditions.

Simulation Experiment 2: In the confined environment, interference was introduced from dynamically moving vehicles (Figure 9). Figure 9a corresponds to the condition“CILQR-30”, while Figure 9b corresponds to the condition“CILQR-60”. The adapted CILQR generates smooth and safe trajectories while maintaining consistency with the global path.

Figure 10a shows that the curvature varies smoothly, with the peak constrained within 0.20 rad/m, satisfying the requirements for driving stability. Figure 10b indicates that the algorithm achieves smoothing by dynamically adjusting the upper bound of the curvature variation rate. Figure 10d confirms that the minimum distance between the path and the obstacles remains within the safe range.

Taking the scenario “CILQR-30” in Simulation Experiment 1 as an example, the total number of path points is 195, and the initial solution lies outside the constraint domain (colliding with obstacles). The adapted CILQR requires 1.5 ms per iteration, with a total computation time of 0.3 s, whereas global re-planning using the A* algorithm takes 8.62 s. The adapted CILQR achieves a 96.6% improvement in computational efficiency, significantly enhancing real-time performance.

5.2. Field Test Validation

Field experiments were conducted on an intelligent tracked vehicle platform, as shown in Figure 11. The platform comprises a tracked mobile chassis (1.00 m × 0.70 m × 0.35 m) with dual CAN bus communication for motion control and feedback, a high-precision integrated INS/GPS system (centimeter-level accuracy) and an upper-level industrial computer (Ubuntu 20.04 LTS, Ryzen 9 8945HX @ 2.5 GHz) for real-time planning and control. The optimal control commands are computed on the upper unit and transmitted to the lower execution unit via the CAN bus, where the chassis receives input to the left/right track velocity and returns real-time feedback. Crucially, the system adopts an asynchronous hierarchical control architecture to handle the different time scales of planning and control. While the upper-level planner updates the trajectory with a cycle of approximately 400 ms (2.5 Hz), the lower-level tracking controller operates at a high frequency (50 Hz). This frequency decoupling ensures that the vehicle maintains precise, real-time response capabilities between planning updates, effectively mitigating the latency impact for the target speed of 0.5 m/s.

Table 3. Experimental parameters of the field test.

Parameter	Unit	Value
$p_{start}$	-	(1, 1)
$p_{goal}$	-	(10, 6)
Grid Size	m	0.10
R	m	1.0
$K_{safe}$	m	0.80
$R_{ego}$	m	1.0
$L_{ego}$	m	1.1
$W_{ego}$	m	0.7

presents the parameter configurations employed in the field test; all parameters not listed here remain identical to those specified in Table 1 and Table 2.

Based on the autonomous obstacle-avoidance scenarios in the simulations, an environment map was constructed from the actual test site to conduct joint experiments of hierarchical motion-planning and hybrid tracking-control methods. Figure 12 presents the experimental results under the autonomous obstacle-avoidance scenario.

In the experimental scenario, Figure 12a includes static obstacles (two polygons and the black polygon on the right), dynamic obstacles (small black polygon), the rough global path (yellow solid line), the smooth global path (blue dashed line), the local planned CILQR path (red dashed line), and the actual trajectory of the LQR-MPC controller (red rectangles). The initial coordinates of the tracked vehicle are (0.5, 1.0), with the standalone LQR and MPC controllers as baselines. Figure 12a,b show that the planned and executed paths are collision-free, while Figure 12c–f demonstrate that the tracking errors of the LQR-MPC hybrid controller converge progressively.

Table 4. Experimental results of autonomous obstacle avoidance.

Controller	${\mathit{X}}_{\mathit{init}}$	${\mathit{U}}_{\mathit{init}}$	${\overline{\mathit{x}}}_{\mathit{e}}$	${\overline{\mathit{y}}}_{\mathit{e}}$	${\overline{\mathit{\theta }}}_{\mathit{e}}$
LQR-MPC	(0.5, 1.0, 0)	(0, 0)	$5.7\times {10}^{-2}$	$2.5\times {10}^{-2}$	$1.1\times {10}^{-2}$
LQR	(0.5, 1.0, 0)	(0, 0)	$5.7\times {10}^{-2}$	$2.7\times {10}^{-2}$	$1.1\times {10}^{-2}$
MPC	(0.5, 1.0, 0)	(0, 0)	$6.9\times {10}^{-2}$	$5.9\times {10}^{-1}$	$2.2\times {10}^{-1}$

summarizes the experimental results in real vehicles of the three controllers under the autonomous obstacle-avoidance scenario, demonstrating the superiority of the proposed LQR-MPC hybrid control method.

The analysis of Table 4 shows that the LQR-MPC hybrid controller achieves an average lateral error of 5.7 cm and an average longitudinal error of 2.5 cm, both smaller than those of the LQR controller (5.7 cm and 2.6 cm) and the MPC controller (6.9 cm and 2.2 cm). While the accuracy gain over standalone LQR is marginal in this specific scenario, the hybrid controller offers superior robustness. Unlike the unconstrained LQR, the hybrid approach explicitly enforces actuator constraints, preventing saturation and ensuring system safety in practical engineering applications.

The joint test under the autonomous obstacle-avoidance scenario demonstrates that all three controllers successfully achieved collision-free path tracking. The hierarchical motion-planning method proposed in this study generates safe, collision-free, and trackable paths. Among the controllers, the LQR-MPC hybrid controller exhibits the smallest average lateral and longitudinal errors, highlighting its superior tracking performance and control accuracy.

6. Conclusions

This study demonstrates the application of a hierarchical motion-planning and tracking-control method to intelligent tracked vehicles, based on an adapted CILQR framework. In the motion-planning layer, a grid network map is constructed with KD-Tree indexing and global path optimization is achieved using an A* algorithm with Chebyshev distance. The improved discrete quadratic smoothing and adapted CILQR algorithms are further integrated to generate collision-free, multi-objective optimized paths in dynamic environments. In the tracking control layer, based on the discrete error model and vehicle tracking requirements, an LQR-MPC hybrid controller with a smooth switching strategy is designed, effectively combining the advantages of different controllers.

Simulation studies demonstrate the feasibility and real-time performance of the proposed hierarchical planning and control framework. Furthermore, real-world autonomous obstacle-avoidance experiments confirm that the proposed method ensures safe and feasible path planning while achieving high-precision tracking control for intelligent tracked vehicles.

Despite promising results, this study has limitations that guide our future research. First, in terms of system architecture, the current validation was limited by laboratory conditions and lacked a fully integrated perception system. Future work will focus on developing onboard perception modules to conduct joint debugging in complex environments. Second, regarding the model validity, the current kinematic model is primarily effective for low-speed (0.5 m/s) and flat-terrain scenarios. Under high-speed conditions, dynamic factors such as significant slip become critical. Future research will incorporate vehicle dynamics to extend the framework’s applicability to high-speed maneuvering and uneven terrain. Finally, regarding algorithm extensibility, while this study focuses on input constraints, the proposed framework demonstrates good potential for expansion. Future applications can readily incorporate velocity limits or other state constraints into the MPC formulation—preferably as soft constraints to maintain optimization feasibility—to adapt to more complex driving scenarios.