Trajectory Control for Car-like Mobile Robots via Frugal Predictive Control with Integrated Disturbance Rejection

Abstract

This paper presents a hierarchical control architecture for high-precision trajectory tracking of a car-like mobile robot (CLMB) operating under external disturbances arising from normal and tangential wheel forces. The proposed solution addresses the critical challenge of simultaneously rejecting disturbances and accurately following a predefined path at a determined cruise velocity. Since the vehicle is equipped with an electronic differential at the low level, a nonlinear dynamic control (NDC) scheme is implemented to regulate the speed in each wheel. This controller actively estimates and compensates for differential traction losses and other lumped disturbances in real time, ensuring robust wheel velocity tracking across varying terrain conditions. The compensated system is then governed by a high-level frugal model predictive controller (FMPC) that leverages a dynamic vehicle model to compute optimal steering and velocity commands, thereby minimizing future trajectory-tracking errors. To achieve a precise and reliable state estimation necessary for feedback control, an Extended Kalman Filter (EKF) is designed to fuse high-frequency data from wheel encoders with absolute pose measurements from a motion capture system, mitigating the drift inherent in odometry alone. Experimental results on a physical robotic platform demonstrate tracking accuracy and robust disturbance rejection under different operating conditions.

1. Introduction

1.1. Preliminaries

Mobile robotics has undergone an unprecedented evolution, driven by the need to automate tasks in sectors as diverse as logistics, precision agriculture, and autonomous transportation [1,2,3,4]. However, achieving full autonomy remains an open challenge, constrained by the vehicles’ ability to perceive, localize, and navigate safely in dynamic and uncertain environments [5,6]. This challenge extends beyond general mobile robotics into the automotive sector, specifically regarding autonomous electric vehicles [7,8,9]. In this context, vehicles with car-like kinematic configurations represent a critical case study due to their widespread adoption in industry and their non-holonomic constraints, which introduce additional complexity to the planning and execution of maneuvers [10,11,12].

Since its initial formulations, the problem of optimal estimation in dynamic systems has been addressed through recursive models that combine sensor data with system predictions. The seminal work of Kalman [13] established the foundation for the filter that bears his name, offering an optimal solution for linear systems with Gaussian noise, characterized by its computational efficiency and recursive nature, Subsequently, this concept has been extended to accommodate the complexities of nonlinear systems [14], this is fundamental for modern robotics where sensor models and vehicle kinematics rarely follow linear assumptions. The recent literature identifies that robust navigation in complex scenarios—where global positioning signals are often unreliable—demands the integration of multisensory architectures capable of mitigating uncertainty [2,6,10]. In this field, sensor data fusion through the Kalman Filter (KF) and its extensions for nonlinear systems, such as the Extended Kalman Filter (EKF), has consolidated as the de facto standard for ensuring consistent state estimation and precise obstacle avoidance [1,3]. Current studies demonstrate that the combination of trilateral localization techniques with advanced filtering allows mobile robots to operate successfully even in chaotic and semi-open environments, laying the groundwork for next-generation industrial and service applications [15]. Alternatively, for outdoor environments, Ref. [16] presents a method to combine Global Navigation Satellite System (GNSS) and BeiDou Navigation Satellite System (BeiDou) data with a low-cost Inertial Measurement Unit (IMU) sensor using an EKF to enhance positioning accuracy. Integrating multiple GNSS systems with a Micro–Electro-Mechanical System Inertial Measurement Unit (MEMS IMU) facilitates achieving centimeter-level precision, which is essential for precision agriculture, autonomous vehicles, and surveying. Recently, the literature has explored the use of deep learning techniques, such as Recurrent Neural Networks (RNNs), to model complex dynamic uncertainties that linear filters fail to capture [17]; however, Wang et al. [18] present a comparative study demonstrating that the EKF maintains a critical superiority in terms of computational efficiency and predictability for real-time vehicle control. In contrast, AI-based approaches [19] often require significant computational load and large volumes of training data.

Once the positioning problem has been resolved, the next challenge consists of precise trajectory tracking. The robot is subject to non-holonomic constraints, and the robotic system is nonlinear and underactuated. These characteristics increase the control design challenge [20]. Various control strategies have been developed to solve this problem, ranging from classical controllers and geometric algorithms to optimal and robust controllers, and, recently, the incorporation of artificial intelligence [21]. In [22,23], control structures based on PID are proposed to achieve trajectory tracking; however, the optimality of the control signal is a factor to consider. In [24], an LQR controller is proposed that better adapts to the lateral tracking control problem of intelligent vehicles under complex operating conditions. The robustness of the control against external perturbations or unmodeled dynamics is crucial in practical implementations; based on that assumption, Ref. [25] presents a backstepping second-order sliding mode control so that the kinematic error states, after a nonlinear transformation, tend to zero in finite time. Given the difficulty of establishing a precise vehicle model, the perturbations to which it may be subjected, and the dynamics it undergoes, control alternatives based on observers [26,27,28,29,30] and the use of AI [31,32]) have emerged. Recently, Model Predictive Control (MPC) has emerged as a robust solution thanks to its ability to anticipate future states and explicitly manage state and input constraints [33]. Various hybrid control schemes based on MPC have been developed [34,35,36] to address deficiencies in the MPC model. Nevertheless, a trade-off exists between the use of nonlinear models (NMPC), which offer a more realistic representation but incur high computational cost and optimization complexity, and linearized models, which facilitate real-time implementation and guaranty more efficient convergence in practical applications [37].

To optimize the implementation of Model Predictive Control (MPC), this work utilizes the concept of feedback linearization, which involves a change in the coordinates applied to the kinematic model of a car-like robot. This approach has been previously explored in the development of various control strategies [38,39,40,41,42]. By employing this method, the system can be simplified to a structure consisting of two simple integrators for the trajectory coordinates in the plane [12,43,44]. This decoupled model enables the MPC optimizer to address the control problem [45]. Subsequently, the calculated control signals are transformed into the vehicle’s physical variables using the inverse coordinate transformation, ensuring correspondence between kinematic prediction and execution.

Translating trajectory control signals into traction system involves careful consideration of the vehicle’s mechanics, such as whether it is two-wheel or four-wheel drive [46]. For example, rear-wheel differential systems [47] use Ackermann–Jeantand geometry to determine the reference velocities for each motor. This electronic differential maintains a consistent velocity difference between the inner and outer wheels during turns, helping prevent slippage and maintaining motion integrity. In terms of alternative methods, Ref. [48] proposes a mechanical system of coupled pedals that enables independent control of wheel speed. In [49], a differential torque control method is proposed; however, the complexity of modeling both the vehicle and the motor remains a significant challenge. To tackle this issue, the work presented in [50] introduces a control law that focuses on regulation error. This approach utilizes low-pass and high-pass filtering of the control signal to enhance performance.

Effective displacement towards target setpoints within a trajectory-tracking framework is achieved by coordinating steering control with the regulation of traction motor angular velocity. To improve system performance in the face of environmental uncertainties, a disturbance estimator can be integrated into the control law [51,52,53,54,55]. This integrated approach enhances robustness, ensuring stable and precise trajectory tracking even under challenging real-world operating conditions.

The experimental phase and validation of theoretical results are crucial in research. The engineering challenge involves developing and effectively integrating systems to close the gap between theory and practice. In the field of mobile robotics, Ref. [56] outlines the fabrication process for various mobile robots platforms. Additionally, Ref. [57] presents an integrated framework for indoor localization along with experimental validation using an autonomous ADAS-based model car. This vehicle determines its global position by merging data from fiducial markers, inertial sensors, and wheel odometry. Moreover, Ref. [58] introduces a testbed designed to evaluate control strategies under outdoor conditions, offering a cost-efficient and repeatable alternative to full-scale vehicle testing.

1.2. Contribution

The primary contribution of this work is the solution’s frugality, which transforms a complex problem—specifically, the trajectory tracking of a car-like mobile robot equipped with an electronic differential—into a modular system incorporating a multi-level hierarchical control scheme. This approach presents a well-considered reformulation that enables the resolution of a nonlinear tracking problem while maintaining the computational efficiency of the quadratic programming (QP) problem associated with linear Model Predictive Control (MPC). Additionally, to decrease the number of decision variables in the QP-problem, a control parameterization method is utilized. This choice decreases the complexity of the optimization problem without compromising the performance of the closed-loop system.

The following list summarizes our contributions:

• Hierarchical integration: We present a modular framework that combines an Extended Kalman Filter (EKF), a Frugal Model Predictive Control (FMPC), and an electronic differential. In this framework, the velocity of each wheel is independently regulated by a nonlinear dynamic control law capable of addressing model uncertainties and external disturbances.
• Nonlinear-to-linear structure via coordinate transformation: Although the kinematics of the car-like mobile robot are governed by a nonlinear model with non-holonomic constraints, we propose controlling the position of a virtual point (“nose”) located at a distance p along the line that is normal to the wheel axis. The kinematics associated with the “nose” position are holonomic, so we can apply a coordinate transformation that effectively decouples the system into two first-order integrators. Controlling the nose position is practically relevant, as this location can accommodate a suite of sensors and a camera.
• MPC based on an integrator mode: Unlike conventional nonlinear MPC (NMPC) the predictive model used in the present work consists of two pure integrators. This transformation leads to a quadratic programming (QP) problem with minimal computational burden, retaining the advantages of linear MPC (e.g., convexity and efficiency) while handling the nonlinear system.
• Control parametrization: To reduce the number of decision variables in the quadratic programming (QP) problem, a reduced control parametrization is adopted. With such a choice, the optimization problem’s complexity is reduced without compromising the closed loop’s performance.
• Experimental validation: To experimentally validate the proposed strategy, a simple yet complete experimental platform is developed using: (1) open and standardized communication protocols, (2) a on-board embedded system to drive wheel angular velocities and direcction, and (3) a MATLAB-ROS (Robot Operating System) ground station.
• Realiability analysis: The reliability of the control strategy was assessed by evaluating its ability to track the desired trajectory, reject disturbances, and meet constraints related to both the state and control. Additionally, we examined the capability of the Extended Kalman Filter (EKF) to integrate relative measurements (odometry) and absolute measurements (position data obtained from motion capture) at various sampling times. Performance was quantified using three well-established indices: the Integral of the Squared Error (ISE), the Integral of the Absolute Error (IAE), and the Integral of Time-weighted Absolute Error (ITAE). The results highlight the effectiveness, reliability, and significance of the proposed strategy.

The remainder of this paper is organized as follows: Section 2 presents the mathematical preliminaries and the problem statement. Section 3 focuses on the control design and estimation design. Section 4, Section 5 and Section 6 describe the development of the experimental platform and its main components, as well as the corresponding results. Finally, Section 7 presents the conclusions.

2. Mathematical Preliminaries and Problem Statement

2.1. Car-like Mobile Robot

Consider a car-like mobile robot as shown in Figure 1. It is important to note the reduction in the geometric representation from four to two wheels (bicycle model): one for traction (rear wheel) and the other for direction (front wheel). To establish the mathematical equations for the kinematic model, the reference point for vehicle position is located at the midpoint of the rear axle. Finally, the kinematic bicycle model is defined as follows:

$${\Sigma }_q:=\left\{\begin{array}{c}\dot{x}=v_x\cos \left(\theta \right)\hfill \\ \dot{y}=v_x\sin \left(\theta \right)\hfill \\ \dot{\theta }={\displaystyle \frac{v_x}{\overline{L}}}\tan \left(\delta \right)\hfill \end{array}\right.$$

(1)

where $v_x$ is the longitudinal velocity of the vehicle, $\delta$ is the steering angle, and $\overline{L}$ corresponds to the wheelbase. Furthermore, let us define the angular velocity of the frame ${\mathbf{E}}^b$ with respect to ${\mathbf{E}}^f$ by $\omega :=\dot{\theta }$.

Figure 1. Kinematic bicycle model for car-like mobile robot.

In trajectory-tracking control for a car-like mobile robot using the kinematics model, it is well known that the system’s control signals are the longitudinal velocity $v_x$ and the steering angle $\delta$. From Figure 1 the front wheel angle $\delta$ can be defined as a function of $v_x$ and $\omega$ (vehicle’s angular velocity) in the form

$$\begin{array}{c}\hfill \delta ={\tan }^{-1}\left({\displaystyle \frac{\omega \overline{L}}{v_x}}\right)\end{array}$$

(2)

In this work, the mobile robot configuration uses Ackermann steering and differential traction on the rear axle, i.e., the left and right wheels are coupled to independent electric motors. Let us define the notation for the angular velocity of each wheel as

$$\begin{array}{c}\hfill {\omega }_i,\mathrm{with}i\in \{L,R\}\end{array}$$

(3)

Using this traction configuration, each wheel can rotate at a different speed without a mechanical differential; additionally, the tire torque can be controlled independently, providing greater stability during various maneuvers and under different road conditions. The longitudinal displacement velocity of the mobile robot is established as a function of the angular velocities of the wheels:

$$\begin{array}{c}\hfill v_x=f({\omega }_L,{\omega }_R)={\displaystyle \frac{r}{2}}\left({\omega }_L+{\omega }_R\right)\end{array}$$

(4)

where r denotes the radius of the wheels of the mobile robot.

2.2. Electronic Differential Based on Ackermann Geometry

Based on the Ackermann steering geometry, we can derive the angular velocities of the inner and outer wheels for curved trajectories. This kinematic model is designed for low-speed operation and assumes that lateral forces and longitudinal tire slip can be neglected. As illustrated in Figure 2, the model relates the steering angles (${\delta }_L$ and ${\delta }_R$) to the radius of rotation ($\overline{R}$). This relationship allows us to determine the appropriate angular speeds (${\omega }_L$ and ${\omega }_R$) for the drive wheels.

Figure 2. Ackermann geometry with electronic differential [59].

Steering angles can be analytically expressed as functions of the turning radius $\overline{R}$ and the wheelbase $\overline{L}$ by means of vehicle kinematic constraints through the following trigonometric relationships:

$$\begin{array}{c}\hfill {\delta }_L={\tan }^{-1}\left({\displaystyle \frac{\overline{L}}{\overline{R}+\frac{d}{2}}}\right),{\delta }_R={\tan }^{-1}\left({\displaystyle \frac{\overline{L}}{\overline{R}-\frac{d}{2}}}\right)\end{array}$$

(5)

where d corresponds to the width of the track. Under the small-angle approximation, the left and right steering angles can be represented by a single nominal parameter, hereafter referred to as the average Ackermann steering angle ($\delta$):

$$\delta ={\displaystyle \frac{{\delta }_L+{\delta }_R}{2}},\tan \left(\delta \right)={\displaystyle \frac{\overline{L}}{\overline{R}}}$$

(6)

The angular velocity of each drive wheel is defined as a function of the vehicle’s longitudinal velocity $v_x$ and the Ackermann steering angle $\delta$. This mathematical relationship enables the implementation of an electronic differential, a control strategy that provides independent speed regulation for the rear traction wheels to ensure coordinated cornering.

$${\omega }_L={\displaystyle \frac{v_x}{r}}\left(1-\left({\displaystyle \frac{d}{2}}{\displaystyle \frac{\tan \left(\delta \right)}{\overline{L}}}\right)\right)$$

(7)

$${\omega }_R={\displaystyle \frac{v_x}{r}}\left(1+\left({\displaystyle \frac{d}{2}}{\displaystyle \frac{\tan \left(\delta \right)}{\overline{L}}}\right)\right)$$

(8)

The calculated angular velocities, ${\omega }_L$ and ${\omega }_R$, serve as reference setpoints for low-level motor controllers. This coordinated control architecture is essential for preventing tire scrub and ensuring the stability of the vehicle’s trajectory before the state estimates are refined by the Extended Kalman Filter.

Assumption 1.

In this work, the geometric parameters of the wheelbase $\overline{L}$, wheel radius r, and track width d are considered known and constant.

2.3. Frugal Model Predictive Control (FMPC)

The FMPC is a specialized MPC framework designed for real-time execution on platforms with limited on-board computational power. Unlike traditional MPC, which can be computationally expensive due to high-dimensional optimization, FMPC utilizes a “frugal” design flow that combines simplified internal models with aggressive control parametrization to drastically reduce the number of decision variables in the underlying optimization problem [60,61].

2.3.1. Core Methodology

At each sampling instant $k\in \mathbb{N}$, the FMPC strategy computes the control sequence

$$\tilde{\mathbf{u}}\left(k\right)\triangleq {\left[{\mathbf{u}}_p^T\left(k\right),{\mathbf{u}}_p^T(k+1),\dots ,{\mathbf{u}}_p^T(k+N-1)\right]}^T\in {\mathbb{R}}^{N{n}_u},$$

(9)

that minimizes a quadratic cost over a finite prediction horizon $N\in {\mathbb{N}}_{+}$. The internal predictor is a discrete-time linear integrator-chain model of the car-like mobile robot, decoupled along its translational degrees of freedom. The cost is defined by

$$J\left({\tilde{\mathbf{u}}}_p\left(k\right)\right)={\displaystyle \sum _{i=1}^N}{∥{\mathbf{C}}_r\mathbf{x}(k+i)-{\mathbf{y}}_d(k+i)∥}_{{\mathbf{R}}_w}^2+{\displaystyle \sum _{i=0}^{N-1}}{∥{\mathbf{u}}_p(k+i)-{\mathbf{u}}_d∥}_{{\mathbf{Q}}_w}^2,$$

(10)

where $\mathbf{x}(k+i)\in {\mathbb{R}}^n$ is the predicted state, ${\mathbf{C}}_r\in {\mathbb{R}}^{n_r\times n}$ selects the regulated outputs, ${\mathbf{y}}_d(k+i)\in {\mathbb{R}}^{n_r}$ is the reference trajectory, ${\mathbf{u}}_d\in {\mathbb{R}}^{n_u}$ is the steady-state input, and ${\mathbf{R}}_w\succ 0$ and ${\mathbf{Q}}_w\succ 0$ are the tracking-error and control-effort weighting matrices, respectively. The minimization of J is subject to:

• Model dynamics;
• Actuator saturation: ${\mathbf{u}}_p^{\min }\le {\mathbf{u}}_p(k+i)\le {\mathbf{u}}_p^{\max }$, $i=0,\dots ,N-1$;
• Output safe set: ${\mathbf{y}}^{\min }\le {\mathbf{C}}_r\mathbf{x}(k+i)\le {\mathbf{y}}^{\max }$, $i=1,\dots ,N$.

The pointwise saturation bounds ${\mathbf{u}}_p^{\min },{\mathbf{u}}_p^{\max }\in {\mathbb{R}}^{n_u}$ and the safe-set bounds ${\mathbf{y}}^{\min },{\mathbf{y}}^{\max }\in {\mathbb{R}}^{n_r}$ vary by the operating envelope of the situation.

2.3.2. Frugal Implementation via Control Parametrization

The online solution of the resulting Quadratic Optimization Problem (QOP) must complete within the available computation budget $T_c<T_s$, where $T_s$ denotes the controller sampling period and $T_c$ the maximum admissible solver time. To enforce this constraint, FMPC adopts a reduced control parametrization: rather than optimizing the $N·n_u$ entries of $\tilde{\mathbf{u}}\left(k\right)$ independently, the entire sequence is expressed as an affine map of a compact decision vector ${\mathbf{p}}_f\left(k\right)\in {\mathbb{R}}^{n_f}$ through a parametrization matrix ${\mathbf{\Pi }}_f\in {\mathbb{R}}^{N·n_u\times n_f}$,

$$\tilde{\mathbf{u}}\left(k\right)={\mathbf{\Pi }}_f{\mathbf{p}}_f\left(k\right),n_{p_f}\ll N·n_u.$$

Using a change of variable and the constraints transforms the original QOP into a reduced form of dimension $n_f$, which can be evaluated with a fraction of the original arithmetic cost while preserving tracking performance. As reported by Vásquez-Cruz et al. [61] for a quadrotor simulation, this reduction can exceed an order of magnitude—for instance, from $N·n_u=90$ to $n_q=6$ decision variables—without measurable degradation of the closed-loop response.

2.3.3. Feedforward Integration

To minimize steady-state trajectory-tracking error, a feedforward term is incorporated into the control signals. This pre-compensation is derived directly from the desired trajectory dynamics, specifically from its derivative. This term is integrated into the FMPC cost function to ensure that the physical constraints of the actuator are respected despite the additional feedforward action. Consequently, the computational overhead of the controller is reduced, allowing the optimization process to focus primarily on position error correction while maintaining overall computational efficiency.

2.4. Kalman Filter

In the field of autonomous systems, pose estimation (cartesian coordinates and angular orientation) for ground-based robotic platforms represents a fundamental challenge in state estimation. The Extended Kalman Filter (EKF) provides a primary recursive mechanism for harmonizing the a priori state estimate derived from internal kinematic models (odometry) with a posteriori stochastic observations obtained from heterogeneous sensor system. For a car-like mobile robot [62], the state vector is typically defined as $\mathbf{q}\in {\mathbb{R}}^3$, where $\mathbf{q}={[x,y,\theta ]}^T$. The EKF propagates the state and its associated uncertainty by linearizing the system dynamics via a first-order Taylor series expansion. In general, the EKF consists of two stages: prediction and correction.

2.5. Problem Statement

As described above, the platform for this research is a car-like mobile robot with differential traction. The main objective is to develop a control strategy to solve the trajectory-tracking problem, ensuring the system follows a parameterized route ${\mathbf{q}}_d\left(t\right)$ over time while maintaining its dynamic constraints on the state and the magnitude of the control signals. Let ${\mathbf{q}}_p={\left(x_p\left(t\right)y_p\left(t\right)\right)}^T\in {\mathbb{R}}^2$ be the coordinates of the point P along the axis ${\mathbf{X}}_B$ in the body frame ${\mathbf{E}}^b$, but expressed in the inertial frame ${\mathbf{E}}^f$. Moreover, let ${\mathbf{u}}_p\left(t\right)={\left(u_x\left(t\right)u_y\left(t\right)\right)}^T\in {\mathbb{R}}^2$ be the velocity of ${\mathbf{q}}_p\left(t\right)$. Then, the control objective is to find ${\mathbf{u}}_p$ such that

$${\mathbf{q}}_e\left(t\right)={\mathbf{q}}_d\left(t\right)-{\mathbf{q}}_p\left(t\right)\to \mathbf{0}\mathrm{when}t\to \infty$$

(11)

subject to the following constraints:

$$\begin{array}{ccc}\mathrm{Position}:\hfill & & {\mathbf{q}}_p^{\min }\le {\mathbf{q}}_p\left(t\right)\le {\mathbf{q}}_p^{\max },\forall t>0,\end{array}$$

(12)

$$\begin{array}{ccc}\mathrm{Velocity}:\hfill & & \hfill {\mathbf{u}}_p^{\min }\le {\mathbf{u}}_p\left(t\right)\le {\mathbf{u}}_p^{\max },\forall t>0,\end{array}$$

(13)

$$\begin{array}{ccc}\mathrm{Velocity}\mathrm{rate}:\hfill & & \hfill \Delta {\mathbf{u}}_p^{\min }\le \Delta {\mathbf{u}}_p\left(t\right)\le \Delta {\mathbf{u}}_p^{\max },\forall t>0.\end{array}$$

(14)

Furthermore, the state of the system must be estimated from relative and absolute measurements using an Extended Kalman Filter.

For that, one proposes the strategy depicted in Figure 3. The schema is composed of three main blocks:

• A high-level control block is charged to generate the linear and angular velocity profiles while maintaining the specified constraints on the state and maximum velocities given by the user. The core of this stage is a Frugal Model Predictive Control.
• A low-level dynamic control law is charged to achieve and maintain the desired angular velocity in each motor, which is dictated by the high-level control. This control law must reject the disturbances and uncertainties in the traction system model.
• An Extended Kalman Filter fuses the odometry, sampled at $\Delta t$ s, and the position measurement obtained by the motion capture system, sampled at $\gamma \Delta t$, with $\gamma \in {\mathbb{N}}_{+}$.

Figure 3. Proposed control architecture.

Figure 3. Proposed control architecture.

3. Control and State Estimation Strategy

In this section, each block of the general schema shown in Figure 3 is described in detail.

3.1. FMPC Applied to the Car-like Mobile Robot

The kinematic model of a car-like mobile robot defined in (1) is a nonlinear system of equations; movement of the body is described by trigonometric functions of the orientation angle $\theta$. Besides, because the center of mass is set at the rear axle, the vehicle cannot have instantaneous lateral movement (non-holonomic restriction). To avoid this limitation, the reference point is moved p along the vehicle. The objective is to obtain a function that describes the relationship between the new reference point P and the state of the system $\mathbf{q}={\left[xy\theta \right]}^T$. The coordinates of P with respect to the inertial frame ${\mathbf{E}}^f$ are given by

$${\mathbf{q}}_p=\left[\begin{array}{c}{x}_p\\ y_p\end{array}\right]=\left[\begin{array}{c}x\\ y\end{array}\right]+p\left[\begin{array}{c}\cos \left(\theta \right)\\ \sin \left(\theta \right)\end{array}\right]$$

(15)

the new point depends on the orientation $\theta$. To determine how that point moves over time, the new position expressions are differentiated. From Figure 4, the displacement of the new point P on the inertial frame is described by

$${\dot{\mathbf{q}}}_p\left(t\right)=\left[\begin{array}{c}{\dot{x}}_p\left(t\right)\\ {\dot{y}}_p\left(t\right)\end{array}\right]=\underset{{\mathbf{J}}_p}{\underbrace{\left[\begin{array}{cc}\cos \left(\theta \right(t\left)\right)& -p\sin \left(\theta \right(t\left)\right)\\ \sin \left(\theta \right(t\left)\right)& p\cos \left(\theta \right(t\left)\right)\end{array}\right]}}\left[\begin{array}{c}{v}_x\left(t\right)\\ \omega \left(t\right)\end{array}\right]$$

(16)

Figure 4. Reference point displaced p distance from rear wheel for feedback linearization.

Let us define the following coordinate change:

$$\left[\begin{array}{c}{v}_x\left(t\right)\\ \omega \left(t\right)\end{array}\right]=\underset{{\mathbf{J}}_p^{-1}}{\underbrace{\left[\begin{array}{cc}\cos \left(\theta \right(t\left)\right)& \sin \left(\theta \right(t\left)\right)\\ -{\displaystyle \frac{1}{p}}\sin \left(\theta \left(t\right)\right)& {\displaystyle \frac{1}{p}}\cos \left(\theta \left(t\right)\right)\end{array}\right]}}\underset{{\mathbf{u}}_p}{\underbrace{\left[\begin{array}{c}{u}_x\left(t\right)\\ u_y\left(t\right)\end{array}\right]}}$$

(17)

where ${\mathbf{u}}_p={\left[u_x{u}_y\right]}^T$ is a control vector that will later be designed to track the reference position. Note that ${\mathbf{u}}_p$ represents the linear velocity of the point P expressed in the inertial frame. Using (17) in (16), gives

$${\Sigma }_{{\dot{q}}_p}:=\left\{\begin{array}{c}{\dot{x}}_p\left(t\right)=u_x\left(t\right)\hfill \\ {\dot{y}}_p\left(t\right)=u_y\left(t\right)\hfill \end{array}\right.$$

(18)

This representation provides a decoupled framework that facilitates the application of linear control techniques while maintaining the essential fidelity of the original kinematic model.

Assumption 2.

The distance p of the virtual point, called “nose,” is constant.

3.1.1. Plant Model for the FMPC

Define the state and control vectors as

$$\mathbf{x}\left(t\right)={\left[x_p\left(t\right)y_p\left(t\right)\right]}^{\top }\in {\mathbb{R}}^2,{\mathbf{u}}_p\left(t\right)={\left[u_x\left(t\right)u_y\left(t\right)\right]}^{\top }\in {\mathbb{R}}^2,$$

(19)

The plant (18) is then rewritten in continuous-time state-space form as

$$\dot{\mathbf{x}}\left(t\right)={\mathbf{A}}_c\mathbf{x}\left(t\right)+{\mathbf{B}}_c{\mathbf{u}}_P\left(t\right),$$

(20)

with

$${\mathbf{A}}_c=\left[\begin{array}{cc}0& 0\\ 0& 0\end{array}\right],{\mathbf{B}}_c=\left[\begin{array}{cc}1& 0\\ 0& 1\end{array}\right].$$

(21)

Note that the pair $({\mathbf{A}}_c,{\mathbf{B}}_c)$ is controllable and the system (20) has the structure on which the FMPC design is based [60].

3.1.2. Discretization by Zero-Order Hold

Let $T_s>0$ be the sampling period. Applying a zero-order hold (ZOH) discretisation scheme to (20) yields the discrete-time model

$$\mathbf{x}(k+1)=\mathbf{A}\mathbf{x}\left(k\right)+\mathbf{B}{\mathbf{u}}_P\left(k\right),$$

(22)

where the discrete matrices are

$$\mathbf{A}=e^{{\mathbf{A}}_c{T}_s}=\left[\begin{array}{cc}1& 0\\ 0& 1\end{array}\right],\mathbf{B}={\int }_0^{T_s}{e}^{{\mathbf{A}}_c\tau }\mathrm{d}\tau {\mathbf{B}}_c=\left[\begin{array}{cc}{T}_s& 0\\ 0& T_s\end{array}\right].$$

(23)

3.1.3. Predictive Scheme

Starting from the model (22), the N-step-ahead state prediction is constructed following the procedure in [60,63]. Concatenating predictions for $i=1,\dots ,N$ yields the compact form

$$\tilde{\mathbf{x}}\left(k\right)=\mathbf{\Phi }\mathbf{x}\left(k\right)+\mathbf{\Psi }\tilde{\mathbf{u}}\left(k\right),$$

(24)

where $\tilde{\mathbf{x}}\left(k\right)\triangleq {\left(\mathbf{x}{(k+1)}^{\top },\dots ,\mathbf{x}{(k+N)}^{\top }\right)}^{\top }$ and $\tilde{\mathbf{u}}\left(k\right)\triangleq {\left({\mathbf{u}}_p{\left(k\right)}^{\top },\dots ,{\mathbf{u}}_p{(k+N-1)}^{\top }\right)}^{\top }\in {\mathbb{R}}^{2N}$ are the stacked state and input sequences over the horizon, and

$$\mathbf{\Phi }\triangleq \left[\begin{array}{c}\mathbf{A}\\ {\mathbf{A}}^2\\ ⋮\\ {\mathbf{A}}^N\end{array}\right],\mathbf{\Psi }\triangleq \left[\begin{array}{c}{\displaystyle {\displaystyle \sum _{j=1}^1}{\mathbf{A}}^{1-j}\mathbf{B}{\mathbf{\Pi }}_j^{(2,N)}}\\ {\displaystyle {\displaystyle \sum _{j=1}^2}{\mathbf{A}}^{2-j}\mathbf{B}{\mathbf{\Pi }}_j^{(2,N)}}\\ ⋮\\ {\displaystyle {\displaystyle \sum _{j=1}^N}{\mathbf{A}}^{N-j}\mathbf{B}{\mathbf{\Pi }}_j^{(2,N)}}\end{array}\right],$$

(25)

with ${\mathbf{\Pi }}_j^{(2,N)}\in {\mathbb{R}}^{2\times 2N}$ being the block-selection matrix that extracts the j-th input from $\tilde{\mathbf{u}}\left(k\right)$, i.e., ${\mathbf{\Pi }}_j^{(2,N)}\tilde{\mathbf{u}}\left(k\right)={\mathbf{u}}_p(k+j-1)$.

The regulated and constrained outputs are the Cartesian positions $x_p$ and $y_p$, selected by means of the following:

$${\mathbf{C}}_r=\left[\begin{array}{cc}1& 0\\ 0& 1\end{array}\right]\in {\mathbb{R}}^{2\times 2},{\mathbf{C}}_c=\left[\begin{array}{cc}1& 0\end{array}\right]\in {\mathbb{R}}^2$$

(26)

The quadratic cost function penalizing position-tracking error and control effort over the horizon N is as was presented in the Equation (10) where ${\mathbf{y}}_d(k+i)\in {\mathbb{R}}^2$. Expanding (10) using (24) and dropping terms that are constant with respect to $\tilde{\mathbf{u}}$ yields the standard quadratic form [63]:

$$J\left(\tilde{\mathbf{u}}\left(k\right)\right)={\displaystyle \frac{1}{2}}\tilde{\mathbf{u}}{\left(k\right)}^T\mathbf{H}\tilde{\mathbf{u}}\left(k\right)+\underset{{\mathbf{F}}^T\left(k\right)}{\underbrace{{\left({\mathbf{F}}_1\mathbf{x}\left(k\right)+{\mathbf{F}}_2{\tilde{\mathbf{y}}}_{\mathrm{d}}+{\mathbf{F}}_3{\mathbf{u}}_{\mathrm{d}}\right)}^T}}\tilde{\mathbf{u}}\left(k\right),$$

(27)

where

$$\begin{array}{cc}\hfill \mathbf{H}& \triangleq 2{\displaystyle \sum _{i=1}^N}\left[{\mathbf{\Psi }}_i^T{\mathbf{C}}_r^T{\mathbf{R}}_w{\mathbf{C}}_r{\mathbf{\Psi }}_i+{\left({\Pi }_i^{(n_{\mathrm{u}},N)}\right)}^T{\mathbf{Q}}_w{\Pi }_i^{(n_{\mathrm{u}},N)}\right],\hfill \end{array}$$

(28)

$$\begin{array}{cc}\hfill {\mathbf{F}}_1& \triangleq 2{\displaystyle \sum _{i=1}^N}\left[{\mathbf{\Psi }}_i^T{\mathbf{C}}_r^T{\mathbf{R}}_w{\mathbf{C}}_r{\mathbf{\Phi }}_i\right],\hfill \end{array}$$

(29)

$$\begin{array}{cc}\hfill {\mathbf{F}}_2& \triangleq -2{\displaystyle \sum _{i=1}^N}\left[{\mathbf{\Psi }}_i^T{\mathbf{C}}_r^T{\mathbf{R}}_w{\Pi }_i^{(n_{\mathrm{u}},N)}\right],\hfill \end{array}$$

(30)

$$\begin{array}{cc}\hfill {\mathbf{F}}_3& \triangleq -2{\displaystyle \sum _{i=1}^N}\left[{\left({\Pi }_i^{(n_{\mathrm{u}},N)}\right)}^T{\mathbf{Q}}_w\right].\hfill \end{array}$$

(31)

Note that $\mathbf{H}$ is computed offline once for all, while $\mathbf{F}\left(k\right)$ is updated at every sampling instant k using the current state $\mathbf{x}\left(k\right)$ and the future reference sequence ${\tilde{\mathbf{y}}}_d\left(k\right)$.

The constrained optimal control sequence is the solution of the quadratic programming problem (QOP),

$${\tilde{\mathbf{u}}}^{★}\left(k\right)=\arg \underset{\tilde{\mathbf{u}}\left(k\right)}{\min }J\left(\tilde{\mathbf{u}}\left(k\right)\right),$$

(32)

subject to the hard constraints listed in Table 1:

$$\begin{array}{ccc}\mathrm{Position}:\hfill & & {\mathbf{y}}^{\min }\le {\mathbf{C}}_c\mathbf{x}(k+i)\le {\mathbf{y}}^{\max },i=1,\dots ,N,\hfill \end{array}$$

(33)

$$\begin{array}{ccc}\mathrm{Velocity}:\hfill & & {\mathbf{u}}^{\min }\le {\mathbf{u}}_p(k+i-1)\le {\mathbf{u}}^{\max },i=1,\dots ,N,\hfill \end{array}$$

(34)

$$\begin{array}{ccc}\mathrm{Velocity}\mathrm{rate}:\hfill & & \Delta {\mathbf{u}}_p^{\min }\le {\mathbf{u}}_p(k+i-1)-{\mathbf{u}}_p(k+i-2)\le \Delta {\mathbf{u}}_p^{\max },i=1,\dots ,N.\hfill \end{array}$$

(35)

The receding-horizon law applies only the first element of the optimal sequence:

$${\mathbf{u}}_p\left(k\right)={\mathbf{\Pi }}_1^{(2,N)}{\tilde{\mathbf{u}}}^{★}\left(k\right).$$

(36)

Table 1. Operational constraints for the car-like mobile robot.

Variable	Lower Bound	Upper Bound
$x_p(k+i)$	$x_p^{\min }$	$x_p^{\max }$
$y_p(k+i)$	$y_p^{\min }$	$y_p^{\max }$
$u_x(k+i-1)$	$u_x^{\min }$	$u_x^{\max }$
$u_y(k+i-1)$	$u_y^{\min }$	$u_y^{\max }$

3.1.4. Frugal Control Parametrization

For a prediction horizon N and $n_u=2$ inputs, the full decision vector $\tilde{\mathbf{u}}\left(k\right)\in {\mathbb{R}}^{2N}$ would require solving a QOP with $2N$ variables at every sampling instant. For embedded real-time use, this may exceed the available budget $T_c<T_s$.

To reduce the computational load while preserving tracking performance, a frugal control parametrization is adopted [60,63]. Instead of optimizing independently all $2N$ components, the input sequence is expressed as a linear function of a compact decision vector ${\mathbf{p}}_f\left(k\right)\in {\mathbb{R}}^{n_f}$ through a parametrization matrix ${\mathbf{\Pi }}_f\in {\mathbb{R}}^{2N\times n_f}$:

$$\tilde{\mathbf{u}}\left(k\right)={\mathbf{\Pi }}_f{\mathbf{p}}_f\left(k\right).$$

(37)

A two-node piecewise-constant profile is adopted: the first node value ${\mathbf{p}}_{f,1}\left(k\right)\in {\mathbb{R}}^2$ is held constant over the first half of the horizon, and the second value ${\mathbf{p}}_{f,2}\left(k\right)\in {\mathbb{R}}^2$ over the second half. For N steps split at $n_s=\lceil N/2\rceil$, the parametrization reads as

$${\mathbf{u}}_p(k+i-1)=\left\{\begin{array}{cc}{\mathbf{p}}_{f,1}\left(k\right),\hfill & i\in \{1,\dots ,n_s\},\hfill \\ {\mathbf{p}}_{f,2}\left(k\right),\hfill & i\in \{n_s+1,\dots ,N\},\hfill \end{array}\right.$$

(38)

so that $n_q=n_f·n_u=2\times 2=4$, i.e., the number of decision variables is reduced from $2N$ to just 4, independently of the chosen horizon length. The resulting parametrization matrix is

$${\mathbf{\Pi }}_f=\left[\begin{array}{cc}{\mathbf{I}}_2& {\mathbf{0}}_2\\ ⋮& ⋮\\ {\mathbf{I}}_2& {\mathbf{0}}_2\\ {\mathbf{0}}_2& {\mathbf{I}}_2\\ ⋮& ⋮\\ {\mathbf{0}}_2& {\mathbf{I}}_2\end{array}\right]\in {\mathbb{R}}^{2N\times 4},$$

(39)

where the upper $2n_s$ block-rows correspond to node 1 and the lower $2(N-n_s)$ block-rows to node 2.

Substituting (37) into (27) and the inequality constraints transforms the original QOP into the reduced problem,

$${\mathbf{p}}_f^{★}\left(k\right)=\arg \underset{{\mathbf{p}}_f\left(k\right)}{\min }{\displaystyle \frac{1}{2}}{\mathbf{p}}_f{\left(k\right)}^{\top }{\mathbf{H}}_f{\mathbf{p}}_f\left(k\right)+{\mathbf{F}}_f{\left(k\right)}^{\top }{\mathbf{p}}_f\left(k\right),$$

(40)

subject to ${\mathbf{A}}_{\mathrm{ineq}}^{\left(f\right)}\mathbf{q}\left(k\right)\le {\mathbf{B}}_{\mathrm{ineq}}^{\left(f\right)}\left(k\right)$, where the reduced matrices are

$${\mathbf{H}}_f\triangleq {\mathbf{\Pi }}_f^{\top }\mathbf{H}{\mathbf{\Pi }}_f\in {\mathbb{R}}^{4\times 4},{\mathbf{F}}_f\left(k\right)\triangleq {\mathbf{\Pi }}_f^{\top }\mathbf{F}\left(k\right)\in {\mathbb{R}}^4,$$

(41)

and ${\mathbf{A}}_{\mathrm{ineq}}^{\left(f\right)},{\mathbf{B}}_{\mathrm{ineq}}^{\left(f\right)}$ are obtained by mapping the constraints (33)–(35) through ${\mathbf{\Pi }}_f$ [60]. ${\mathbf{H}}_f$ is computed offline; only ${\mathbf{F}}_f\left(k\right)$ and ${\mathbf{B}}_{\mathrm{ineq}}^{\left(f\right)}\left(k\right)$ need to be updated online. Finally, the receding-horizon control law is recovered as

$${\mathbf{u}}_p\left(k\right)={\mathbf{\Pi }}_1^{(2,N)}{\mathbf{\Pi }}_f{\mathbf{p}}_f^{★}\left(k\right).$$

(42)

Remark 1.

Note that the control ${\mathbf{u}}_p\left(k\right)$ represents the desired linear velocity at the instant k such that the point P in the mobile frame follows the desired trajectory. Then, ${\mathbf{u}}_p\left(k\right)$ must be mapped to the desired angular velocities of each wheel. For that, at the instant k, the vector ${\left[v_x\left(k\right)\omega \left(k\right)\right]}^T$ is computed via ${\mathbf{J}}_{\mathbf{p}}^{-1}$ in (17), and the steering angle $\delta \left(k\right)$ is later derived from (2). Then, using (7) and (8) gives

$${\omega }_L^d\left(k\right)={\displaystyle \frac{v_x^d\left(k\right)}{r}}\left(1-\left({\displaystyle \frac{d}{2}}{\displaystyle \frac{\tan \left(\delta \right(k\left)\right)}{\overline{L}}}\right)\right)$$

(43)

$${\omega }_R^d\left(k\right)={\displaystyle \frac{v_x^d\left(k\right)}{r}}\left(1+\left({\displaystyle \frac{d}{2}}{\displaystyle \frac{\tan \left(\delta \right(k\left)\right)}{\overline{L}}}\right)\right)$$

(44)

The complete FMPC structure for the car-like mobile robot is summarized in Figure 3. The user supplies the position constraints, weighting matrices ${\mathbf{R}}_w$ and ${\mathbf{Q}}_w$, and the reference trajectory; the FMPC block computes ${\mathbf{u}}_p\left(k\right)$ from the reduced QOP at every sampling instant k and issues the corresponding velocity and steering commands to the vehicle.

3.2. Nonlinear Dynamic Control for the Wheel Speed

The wheels in the mobile robot are actuated by DC-motors. Then, the angular velocity dynamics, in continuous-time, for each wheel is given by

$${\dot{\omega }}_i\left(t\right)=-a{\omega }_i\left(t\right)+b{u}_{m_i}\left(t\right)+d_i\left(t\right),\mathrm{with}i\in \{L,R\}$$

(45)

a and b are constants that depend on the electrical and mechanical parameters of the motor and will be assumed to be uncertain in this work. $u_m$ is the applied voltage (control) and $d_i\in \mathbb{R}$ represents a signal that concentrates external disturbances, originating from the normal and tangential forces present in the wheels of the vehicle and reflected as a moment of forces around the rotational axis. In the following, the temporary variable t will be omitted unless the context requires it.

To establish a simple but robust dynamic controller, the dynamics (45) can be further written as

$${\dot{\omega }}_i=\alpha (u_{m_i}-{\xi }_i)$$

(46)

where $\alpha$ and ${\xi }_i\in \mathbb{R}$ are assumed unknown. The control objective is to guaranty that ${\omega }_i\to {\omega }_i^d$, when $t\to \infty$ regardless of uncertainties in the model denoted by $\alpha$, and the disturbance $\xi$ which represents the sum of endogenous and exogenous disturbances and can be a time variant.

Before stating the main result, it is important to establish the following considerations:

• $\alpha >0$ such that $\alpha \in [{\alpha }_{\min },{\alpha }_{\max }]$;
• ${\xi }_i\in [{\xi }_{\min },{\xi }_{\max }]$;
• $u_{m_i}\in [-M,M]$, ∀$M>0$;
• $|{\displaystyle \frac{d{\xi }_i}{dt}}|\le K_0$.

Definition 1

(Saturation function). Given a positive constant M, the continuous and non decreasing function ${\sigma }_M:\mathbb{R}\to \mathbb{R}$ is defined as

$$\begin{array}{cc}\left(1\right){\sigma }_M=s\mathrm{if}\left|s\right|<M;\hfill & \hfill \\ \left(2\right){\sigma }_M=M·\mathrm{sign}\left(s\right); \mathrm{in}\mathrm{other}\mathrm{case}\hfill & \hfill \end{array}$$

(47)

Proposition 1.

Consider the system (46) with the dynamic control law

$$\begin{array}{cc}\hfill {\dot{z}}_i& ={\lambda }_f(u_{m_i}-z_i)\hfill \\ \hfill u_{m_i}& ={\sigma }_M(\lambda ({\omega }_i^d-{\omega }_i)+z_i)\hfill \end{array}$$

(48)

with $\lambda ,{\lambda }_f\in {\mathbb{R}}^{+}$. Let us define $e_x:={\omega }_i-{\omega }_i^d$ and $e_z:=z_i-{\xi }_i$ as the tracking error and the estimation error, respectively. In addition, consider $X_e={\left(e_x{e}_z\right)}^T$ as the state vector in the error’s coordinates. Then, after a long enough time, the error vector converges to the neighborhood of the origin.

$$\parallel X_e\left(t\right)\parallel \le {\displaystyle \frac{K_0}{{\lambda }_M}}$$

(49)

$K_0$ was previously defined and ${\lambda }_M$ is the highest eigenvalue of the matrix $A_s={\displaystyle \frac{1}{2}}(A_e+A_e^T)$, where $A_e$ is given by

$$A_e=\left(\begin{array}{cc}-\alpha \lambda & \alpha \\ -{\lambda }_f\lambda & 0\end{array}\right)$$

(50)

The proof is given in Appendix A.

Remark 2.

Note that this low-level control law, (48), is designed in continuous-time but implemented in discrete-time in an embedded microprocessor running to a frequency higher than the high-level control given by the FMPC.

3.3. Extended Kalman Filter for State Estimation

In navigation applications, whether indoors or outdoors, it is common to utilize multiple sensors (such as encoders, cameras, laser sensors, ultrasonic sensors, IMUs, and GPS) to measure or estimate the vehicle’s position. To reduce trajectory-tracking errors, it is essential for the mobile robot’s current position to be accurate, as this accuracy allows the tracking controller to generate optimal control commands.

The Extended Kalman Filter (EKF) is a version of the Kalman filter designed for nonlinear systems [64]. While the standard filter only works optimally in linear systems, the EKF allows the state of more complex systems to be estimated via linearization. The fusion of sensory data with the Extended Kalman Filter (EKF) is enabled by a two-step iterative process that combines a mathematical model of the system with actual measurements from multiple sensors, assigning a weight to each measurement based on its uncertainty. The algorithm employs the Kalman gain $\left({\mathbf{K}}_{kf}\right)$ to dynamically balance model prediction and sensor measurement based on their respective uncertainties. The state and uncertainty are projected forward using a nonlinear transition model based on the system’s physical laws. This distinguishes it from the standard Kalman Filter, which relies on a linear state-space representation. The discrete-time kinematic bicycle model (1) based on wheel speeds (4), which defines the state ${\mathbf{q}}_{k+1}$, is given by

$$\overline{\mathbf{q}}\left(k\right)=\widehat{\mathbf{q}}(k-1)+\left[\begin{array}{c}\left[r\cos \left(\theta \left(k\right)\right)\right][{\omega }_L\left(k\right)+{\omega }_R\left(k\right)]\Delta t/2\\ \left[r\sin \left(\theta \left(k\right)\right)\right][{\omega }_L\left(k\right)+{\omega }_R\left(k\right)]\Delta t/2\\ \left[r\tan \left(\theta \left(k\right)\right)\right][{\omega }_L\left(k\right)+{\omega }_R\left(k\right)]\Delta t/2\overline{L}\end{array}\right]+\mathbf{w}\left(k\right)$$

(51)

where $\Delta t$ denotes the sampling interval between updates and $\mathbf{w}\left(k\right)$ is the process noise vector, accounting for modeling inaccuracies and external disturbances in the robot’s motion.

Since the EKF operates on a linear approximation of the system dynamics, we linearize at each time step around the current operating point. This allows for the forward propagation of the state uncertainty. The corresponding Jacobian is expressed as

$${\mathbf{F}}_{kf}\left(k\right)={\displaystyle \frac{\partial f[\widehat{\mathbf{q}}(k-1),{\mathbf{u}}_{kf}(k-1)]}{\partial \mathbf{q}}}=\left[\begin{array}{ccc}1& 0& -\left[r\sin \left(\theta (k-1)\right)\right][{\omega }_L(k-1)+{\omega }_R(k-1)]\Delta t/2\\ 0& 1& \left[r\cos \left(\theta (k-1)\right)\right][{\omega }_L(k-1)+{\omega }_R(k-1)]\Delta t/2\\ 0& 0& 1\end{array}\right]$$

(52)

An OptiTrack system provides direct state observations, yielding a linear measurement model. Under the assumption of full state observability, the observation function $h\left({\mathbf{q}}_k\right)$ is defined by

$$\begin{array}{c}\hfill {\mathbf{z}}_{kf}\left(k\right)=h\left(\mathbf{q}\left(k\right)\right)+\mathbf{v}\left(k\right)=\left[\begin{array}{c}x\left(k\right)\\ y\left(k\right)\\ \theta \left(k\right)\end{array}\right]+\mathbf{v}\left(k\right)\end{array}$$

(53)

where ${\mathbf{z}}_{kf}\left(k\right)$ represents the actual measurements at time k, while $\mathbf{v}\left(k\right)$ accounts for the additive measurement noise. To facilitate filter update, the measurement model is linearized about the current predicted state ${\widehat{\mathbf{q}}}_k$ as follows:

$${\mathbf{H}}_{kf}={\displaystyle \frac{\partial h\left(\mathbf{q}\right(k\left)\right)}{\partial \mathbf{q}}}=\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\\ 0& 0& 1\end{array}\right]$$

(54)

The stochastic components of the model, namely the process noise ${\mathbf{w}}_k$ and measurement noise ${\mathbf{v}}_k$, are assumed to be zero-mean additive White Gaussian Noise (WGN). These are characterized by their respective covariance matrices ${\mathbf{Q}}_{\mathbf{kf}}\in {\mathbf{R}}^{3\times 3}$ and ${\mathbf{R}}_{\mathbf{kf}}\in {\mathbb{R}}^{3\times 3}$. In this study, the measurement noise covariance ${\mathbf{R}}_{kf}$ is empirically determined on the basis of the static precision of the OptiTrack system:

$${\mathbf{R}}_{kf}=\left[\begin{array}{ccc}{\sigma }_{opt\_x}^2& 0& 0\\ 0& {\sigma }_{opt\_y}^2& 0\\ 0& 0& {\sigma }_{opt\_\theta }^2\end{array}\right]$$

(55)

The covariance matrix represents the uncertainties in the kinematic model, incorporating errors such as wheel slippage and surface irregularities. Following the approach in [65], this is defined based on the expected variability of control inputs and motion model noise. Under the assumption that the noise in the steering angle and wheel speeds are independent and Gaussian, the matrix is expressed as

$${\mathbf{Q}}_{kf}=\left[\begin{array}{ccc}{\sigma }_x^2& 0& 0\\ 0& {\sigma }_y^2& 0\\ 0& 0& {\sigma }_{\theta }^2\end{array}\right]$$

(56)

where ${\sigma }_x^2$, ${\sigma }_y^2$ and ${\sigma }_{\theta }^2$ are the variance of the prediction states ($x,y,\theta$). The EKF algorithm operates iteratively through the following two main phases.

3.3.1. Prediction

In the first stage, the state vector is projected forward using the nonlinear motion model:

$$\begin{array}{c}\hfill \overline{\mathbf{q}}\left(k\right)=f(\widehat{\mathbf{q}}(k-1),{\mathbf{u}}_{kf}\left(k\right))+\mathbf{w}\left(k\right)\end{array}$$

(57)

The state covariance is propagated through the linearized system dynamics:

$$\begin{array}{c}\hfill \overline{\mathbf{P}}\left(k\right)={\mathbf{F}}_{kf}\left(k\right)\widehat{\mathbf{P}}(k-1){\mathbf{F}}_{kf}^T\left(k\right)+{\mathbf{Q}}_{kf}\end{array}$$

(58)

3.3.2. Update

The innovation vector $\mathbf{y}\left(k\right)$ represents the discrepancy between the actual sensor measurement and the predicted observation:

$$\begin{array}{c}\hfill \mathbf{y}\left(k\right)={\mathbf{z}}_{kf}\left(k\right)-{\mathbf{H}}_{kf}\overline{\mathbf{q}}\left(k\right)\end{array}$$

(59)

Once a measurement is available, the Kalman gain ${\mathbf{K}}_{kf}$ is computed to weigh the reliability of the sensors:

$$\begin{array}{c}\hfill {\mathbf{K}}_{kf}\left(k\right)=\overline{\mathbf{P}}\left(k\right){\mathbf{H}}_{kf}^T{({\mathbf{H}}_{kf}\overline{\mathbf{P}}\left(k\right){\mathbf{H}}_{kf}^T+{\mathbf{R}}_{kf})}^{-1}\end{array}$$

(60)

In the final step, the posteriori state estimate and its associated covariance are refined by means of the calculated Kalman gain:

$$\begin{array}{c}\hfill \widehat{\mathbf{q}}\left(k\right)=\overline{\mathbf{q}}\left(k\right)+{\mathbf{K}}_{kf}\mathbf{y}\left(k\right)\end{array}$$

(61)

$$\begin{array}{c}\hfill \widehat{\mathbf{P}}\left(k\right)=(\mathbf{I}-{\mathbf{K}}_{kf}{\mathbf{H}}_{kf})\overline{\mathbf{P}}\left(k\right)\end{array}$$

(62)

By iteratively applying these prediction and correction phases, the EKF minimizes the estimation error covariance, providing a robust tracking of the vehicle’s trajectory.

4. Experimental Platform Description

To validate the control strategy, an ecosystem was developed for the experimental deployment and analysis of the results. This ecosystem consists of three main components: (1) a real-time localization system (RTLS), (2) a car-like robot equipped with an open-architecture autopilot, and (3) a control station responsible for (i) processing information from the RTLS, (ii) executing the control strategy, and (iii) sending the desired steer angle and the desired angular velocity to each motor of the vehicle so that deployment tasks are executed. Figure 5 shows the described ecosystem.

Figure 5. The experimental platform is divided into four key workspaces: the Wi-Fi communication network, the ground control station, the motion capture (MoCap) system, and the navigation test area. The main components include a local router, a ground station for executing control algorithms, MoCap equipment, and the mobile robot. To view a video of the experiment, please visit https://www.youtube.com/watch?v=MQM11T2g1CE or scan the QR code. Accessed on 1 May 2026.

The ground station features are a 12th-generation Core i7-1255U 1.7 GHz processor, 12 GB RAM, Ubuntu 22.04 LTS, and MATLAB 2024B. The onboard controller operates under the ESP32 Devkit v1 together with the board based on a STM32H7 microcontroller, allowing operation in a 100 Hz control loop.

4.1. RTLS

The localization stage is based on a motion capture (MoCap) system comprising a network of infrared cameras and specialized software that allows real-time monitoring of an object’s movement from light reflected by markers attached to the vehicle. In this case, the Optitrack system was used, which provides the pose, a 6-DoF tracking vector: 3 DoF for position and 3 DoF for orientation of the vehicle. In our case, only 2 DoF for position and 1 DoF for orientation are used.

4.2. Robotic Platform

The physical agent is a car-like mobile robot, characterized by non-holonomic constraints (Ackermann-steering vehicle). It is equipped with two 12V DC encoder motors on the rear axle and a controller on board for speed control and wireless communication. Table 1 details the platform characteristics.

4.3. Control Station

The control station (CS) acts as the system’s central hub, utilizing ROS2 Humble to manage data distribution. It publishes the vehicle’s pose to a high-level FMPC trajectory-tracking controller within the MATLAB 2024B environment. Furthermore, the CS processes wheel speed data through an Extended Kalman Filter (EKF), fusing it with kinematic odometry to provide a robust pose estimation for the car-like robot. All system variables are monitored in real-time through the control station.

4.4. Extended Kalman Filtering Tuning

For computational efficiency, the process noise covariance matrix, ${\mathbf{Q}}_{kf}$, is assumed to be diagonal, where each element represents the error variance of the corresponding state component (Equation (56)). The terms ${\sigma }_x^2$ and ${\sigma }_y^2$ are modeled as proportional to the traveled distance and velocity, whereas ${\sigma }_{\theta }^2$ accounts for uncertainties during rotational maneuvers. Experimental calibration is performed through straight-line motion at a constant velocity to characterize the position error variances ${\sigma }_x^2$ and ${\sigma }_y^2$. Analogously, ${\sigma }_{\theta }^2$ is determined through controlled rotation tests. A high-magnitude ${\mathbf{Q}}_{kf}$ de-weights the odometry model in favor of external correction sensors, whereas a low-magnitude ${\mathbf{Q}}_{kf}$ prioritizes the dead reckoning estimates.

To ensure the reproducibility of the estimation stage, the EKF parameters are provided. The process noise covariance matrix ${\mathbf{Q}}_{kf}$ was set to

$${\mathbf{Q}}_{kf}=\left[\begin{array}{ccc}0.01& 0& 0\\ 0& 0.01& 0\\ 0& 0& 0.01\end{array}\right]$$

(63)

The measurement noise covariance matrix was characterized based on the OptiTrack motion capture system’s precision (with an experimental $error$ < 2 mm):

$${\mathbf{R}}_{kf}=\left[\begin{array}{ccc}0.00001& 0& 0\\ 0& 0.00001& 0\\ 0& 0& 0.00001\end{array}\right]$$

(64)

Regarding the prediction stage, the geometric parameters required for odometry and state estimation are summarized in Table 2.

Table 2. Car-like mobile robot platform parameters.

Parameter	Notation	Value
Wheel base	$\overline{L}$	0.175 m
Wheel radius	r	0.033 m
Track width	d	0.18 m
Virtual point	p	0.08 m
Maximum steering angle	${\delta }_{max}$	0.69 rad
Maximum speed of wheels	${\omega }_{i\mathit{max}}$	37.69 rad/s
Maximum linear velocity	$v_{xmax}$	1.24 m/s
Maximum angular velocity	${\omega }_{max}$	0.94 rad/s

4.5. Experiment Description

To validate the proposed control scheme and evaluate the integrated subsystems, five experiments were designed, each repeated 10 times to assess reliability. In all cases, the objective was to track a time-parameterized lemniscate trajectory. The trajectory is defined by the following parametric equations:

$$\begin{array}{cc}\hfill x_d\left(t\right)& ={\displaystyle \frac{\overline{a}\cos \left({\omega }_{o}t\right)}{{\sin }^2\left({\omega }_{o}t\right)+1}}\hfill \\ \hfill y_d\left(t\right)& ={\displaystyle \frac{\overline{a}\cos \left({\omega }_{o}t\right)\sin \left({\omega }_{o}t\right)}{{\sin }^2\left({\omega }_{o}t\right)+1}}\hfill \end{array}$$

(65)

where $\overline{a}$ and ${\omega }_o$ represent the amplitude of the trajectory and the angular frequency, respectively.

The proposed trajectory-tracking strategy is based on a kinematic bicycle model, focusing exclusively on planar translation velocities and the yaw rate ($\dot{\theta }$). Given that the high-level Model Predictive Control (MPC) employs a kinematic model linearized through input–output feedback linearization (shifted output), dynamic effects such as lateral and rolling forces are inherently excluded from the formulation. Consequently, the operational range is restricted to low-speed maneuvers, where the non-slip assumption remains valid and dynamic perturbations can be effectively mitigated by the low-level controllers, which incorporate disturbance observers for the traction units. Furthermore, the electronic differential algorithm remains applicable under these conditions. To ensure the physical consistency of the zero-slip hypothesis during experimental validation, high-density EVA foam was utilized to provide a high-friction contact surface. The actuator constraints and the steering limits of the car-like mobile robot are summarized in Table 2.

• Experiment 1. The objective of this test is to evaluate the performance of the FMPC relative to a Linear Quadratic Regulator (LQR) using identical cost matrices. Furthermore, trajectory-tracking precision is assessed at a control frequency of 100 Hz, while the EKF-based sensor fusion is validated using a 1 Hz correction rate to test the system’s robustness under low-frequency updates.
• Experiment 2. Leveraging the enhanced capabilities of the EKF-based framework, this test evaluates how state estimation bolsters the robustness of trajectory tracking. The architecture facilitates multi-rate operation, decoupling the high-frequency control execution from the lower-frequency pose measurements. Specifically, the EKF prediction stage and the control loop operate at a 10 ms sampling period, whereas the EKF correction stage is triggered at a 4 s interval to assess the system’s dead-reckoning reliability during sensor latency.
• Experiment 3. A fundamental advantage of Model Predictive Control (MPC) is its inherent capability to incorporate explicit state and control input constraints. This experiment validates this feature by imposing the state constraint $x_{min}\le x_k\le x_{max}$, ensuring the car-like robot’s pose remains within a predefined safe operating range. The boundaries for the linear trajectory were set to $\pm 0.85$ m, while for the circular path, they were defined as $\pm 1.1$ m. To further challenge the controller’s constraint-handling capabilities, the trajectory amplitude $\overline{a}$ was increased to $1.3$ m, exceeding the $1.2$ m used in previous benchmarks.
• Experiment 4. The robustness of the control scheme against external disturbances was evaluated through the low-level controller. This controller features a nonlinear structure integrated with an input-based disturbance observer. To test its ability to regulate wheel speed under variable conditions, an additional payload of 896 g was added to the vehicle. Within the trajectory-tracking framework, the vehicle is required to track a specific velocity profile to ensure spatial accuracy at every time step. Therefore, the low-level control must effectively compensate for the added mass and maintain the velocity within the bounds defined by the high-level trajectory-tracking algorithm.
• A central contribution of this work is the computational efficiency of the control scheme. Specifically, motor control, data fusion for pose estimation, and the trajectory-tracking algorithm are executed within a 10 ms sampling interval, necessitating a low computational overhead. To evaluate the impact of the proposed FMPC parameterization on both execution time and tracking precision, its performance was benchmarked against a conventional, non-parameterized MPC.

5. Experimental Results

5.1. Experiment 1: DLQR vs. FMPC

To validate the proposed control scheme and ensure a fair comparison, a Digital Linear Quadratic Regulator (DLQR) was developed, as both approaches are categorized as optimal control strategies and were tested under identical conditions. The resulting time-series trajectory tracking for each state is presented in Figure 6. The dashed black line represents the reference, while solid lines and shaded areas denote the mean ($\mu$) and confidence intervals ($\pm 2\sigma$) for DLQR (orange) and FMPC (blue), maintaining consistency across all comparative results. It is observed that the FMPC (blue line) exhibits a slightly more pronounced initial transient in the y-axis—an expected behavior due to the imposed constraints—yet it successfully converges to the reference smoothly. Both controllers demonstrate excellent tracking capabilities relative to the reference signals. Furthermore, the uncertainty bands ($\mu \pm 2\sigma$) suggest that the FMPC maintains a competitive variance against the DLQR, which is noteworthy given that the FMPC solves a constrained optimization problem at each time step. The shaded regions in the $\theta$ plot indicate uncertainty peaks during abrupt changes in direction; however, the FMPC effectively manages these high-dynamic regions with a controlled standard deviation.

Figure 6. DLQR vs. FMPC with correction at 1 s.

The Figure 7 illustrates the control effort for the traction motors (${\omega }_L,{\omega }_R$) and the steering angle ($\delta$). The DLQR exhibits higher variance and more aggressive oscillations, which could lead to mechanical fatigue or actuator damage in a physical implementation. Furthermore, the motor velocity profiles for the FMPC are smoother, indicating a more energy-efficient control action compared to the DLQR, while maintaining competitive tracking performance.

Figure 7. Control signals in DLQR vs. FMPC comparison.

The trajectory-tracking performance in the phase space (x-y plane) is shown in Figure 8. As previously identified in the time-series state dynamics, both approaches achieve successful tracking of the reference trajectory, despite the EKF pose correction occurring at a low frequency of 1 Hz. Notably, the FMPC exhibits more reliable behavior due to its iterative optimization process at each sampling instant; consequently, the steady-state covariance once the trajectory is reached is minimal. Furthermore, its predictive horizon allows the system to anticipate the desired path, resulting in a smoother initial response. In contrast, while the DLQR is also capable of ensuring correct trajectory following, its repeatability is lower. This is attributed to its reactive nature when responding to positioning errors, despite its inherent optimal formulation.

Figure 8. The trajectory-tracking performance in the phase space (x-y) plane for DLQR vs. FMPC.

To quantitatively evaluate the tracking performance, three integral error criteria were computed: Integral Square Error (ISE), Integral Absolute Error (IAE), and Integral Time-weighted Absolute Error (ITAE). The resulting metrics are presented in Table 3. It is important to note that these metrics were calculated after an initial transient period (t > 5 s) to ensure that the comparison reflects the controllers’ ability to maintain the trajectory rather than their initial convergence speed. As shown in Figure 9, the FMPC consistently outperforms the DLQR across all metrics. The significantly lower ISE and ITAE values demonstrate that the FMPC achieves higher tracking fidelity and superior convergence stability, effectively mitigating cumulative errors that the reactive DLQR fails to suppress over time.

Table 3. Quantitative evaluation of tracking precision for Experiment 1. The table summarizes ISE, IAE, and ITAE values for both DLQR and FMPC implementations.

Metric	DLQR	FMPC
Mean ISE	0.0627	0.0259
Mean IAE	0.7571	0.5622
Mean ITAE	9.1860	7.0081

Figure 9. ISE, IAE, and ITAE evolution for both DLQR and FMPC with correction at 1 s.

5.2. Experiment 2: Validation of Pose Estimation Under Multi-Rate EKF Correction Frequencies

The complete control architecture is where all the processes are synchronized at a high-frequency sampling rate of 100 Hz (10 ms). This rapid execution cycle ensures seamless interaction between the EKF-based state estimation and the FMPC law. As evidenced by the time-series plots in Figure 10, the Extended Kalman Filter provides a robust, low-latency pose estimate, effectively mitigating sensor noise while maintaining high-fidelity tracking of the vehicle’s rapid dynamics. The actuator responses for the traction motors (${\omega }_L,{\omega }_R$) and the steering angle ($\delta$) demonstrate near-perfect alignment with the required setpoints, even during aggressive maneuvers (as shown in Figure 11).

Figure 10. States evolution along the time of FMPC with EKF correction at 10 ms.

Figure 11. Control signals for FMPC with EKF correction at 10 ms.

Phase-space analysis confirms exceptional trajectory repeatability, characterized by minimal steady-state variance captured in Figure 12. These results definitively validate the real-time feasibility and computational efficiency of the proposed hierarchical scheme on an embedded platform.

Figure 12. Trajectory-tracking performance of control schema with EKF correction at 10 ms.

To evaluate the system’s reliability during sensor degradation or low-frequency positioning updates, the EKF correction stage was throttled to a 4-s interval. As illustrated in Figure 13, Figure 14 and Figure 15, the hierarchical scheme maintains trajectory integrity through extended periods of dead-reckoning. The time-series analysis reveals that the EKF prediction stage successfully bridges the 4-s gaps between measurements, preventing the controller from becoming unstable. Although the phase-space plots show a marginal increase in the confidence intervals ($\pm 2\sigma$)—typical of cumulative odometric drift—the FMPC effectively manages this uncertainty, resulting in a smooth path that converges back to the reference upon each sensor update. These results confirm the high robustness of the proposed estimator–controller pair under extreme feedback latency. Furthermore, this experiment effectively simulates a GPS-denied environment or a scenario with significant sensor occlusion, where the vehicle must rely solely on its internal kinematic model and odometry for extended periods. Despite the 4-s latency, the FMPC demonstrates remarkable smoothness during the state-correction instances. Rather than exhibiting abrupt ’jumps’ or erratic steering behavior when a new measurement is received, the controller leverages its optimization-based nature to seamlessly re-integrate the updated pose into the predictive plan. This ability to maintain dynamic stability under extreme feedback sparsity highlights the reliability of the proposed hierarchical framework for real-world autonomous missions where sensor reliability cannot be guaranteed.

Figure 13. x, y and $\theta$ states evolution at 4 s correction rate.

Figure 14. Car-like mobile robot control signals (wheels’ speeds and steering angle) at 4 s rate correction.

Figure 15. Trajectory-tracking performance of mobile robot at 4 s rate correction.

The comparative performance analysis under variable correction rates is presented in Figure 16 and Table 4, the cumulative error metrics (ISE, IAE, and ITAE) for three distinct EKF correction intervals are 10 ms, 1 s, and 4 s.

Figure 16. Performance for EKF each correction time.

Table 4. Performance metrics (ISE, IAE, and ITAE) of the FMPC-EKF scheme across different pose correction sampling periods.

Metric	4 s Correction	1 s Correction	10 ms Correction
Mean ISE	0.1305	0.0259	0.0221
Mean IAE	1.1510	0.5622	0.4914
Mean ITAE	13.6049	7.0081	6.0795

A key finding is the minimal performance gap between the 10 ms and 1 s correction rates, indicating that the EKF effectively bridges the feedback latency without significant degradation in tracking precision. While the 4 s correction interval (green line) exhibits higher cumulative error—attributable to the prolonged periods of open-loop odometric prediction—it maintains a stable and predictable trend. The fact that the ITAE slope remains bounded even under such sparse feedback conditions validates the robustness of the FMPC-EKF pair, confirming its suitability for autonomous navigation in environments with intermittent sensor availability.

5.3. Experiment 3: State Constraint Handling

Regarding the operational constraints, Figure 17 demonstrates the FMPC’s ability to enforce strict operational boundaries, specifically the lateral state constraint $x_{min}\le x_k\le x_{max}$. In both the linear (a) and circular (b) tracking scenarios, the FMPC successfully confines the robot’s pose within the predefined safe zones (indicated by the red-shaded regions). As observed in the $x\left(t\right)$ plots, whenever the reference trajectory exceeds the safety limits, the controller prioritizes constraint satisfaction over reference tracking, effectively saturating the state at the boundary. The phase-space plots further confirm this behavior, showing how the vehicle’s trajectory is ‘compressed’ to stay within the allowable x-range. This inherent safety feature is critical for autonomous navigation in constrained environments, proving that the proposed hierarchical scheme ensures collision avoidance and regulatory compliance through its active optimization-based framework.

Figure 17. Constrained scenarios. (a) Straight line as a reference, and (b) circle as a reference.

5.4. Experiment 4: Load Disturbance Compensation

In trajectory tracking, precise longitudinal speed control is critical due to the time-dependent nature of the reference trajectory. As illustrated in the upper plots of Figure 18, the wheel angular velocities successfully track the reference signals generated by the electronic differential, even under an 896 g payload. Despite the dynamic effects introduced by the additional mass, the tracking error remains within acceptable margins. This performance is attributed to the low-level controller, which incorporates a control-input-based disturbance observer and explicitly accounts for actuator saturation limits. The lower plots of Figure 18 depict the evolution of the disturbance estimation for both the left and right wheel motors. The observer effectively compensates for the added mass, which is reflected in the increased magnitude of the control signals. Notably, this increase is more pronounced in the left motor; although the payload was intended to be centered, its final placement fell slightly toward the left side. The control scheme successfully identified and compensated for this experimental asymmetry, maintaining system stability. The narrow variance ($\mu \pm 2\sigma$) shown in the shaded regions of Figure 19 indicates high repeatability. Furthermore, while the performance indices (Figure 20) show a cumulative increase in tracking error—particularly the ITAE, which rises more sharply during segments of maximum curvature, the state-phase diagram confirms that this deviation is not significant. Ultimately, the vehicle maintains high fidelity to the lemniscate geometry, demonstrating that the proposed control scheme is robust against both significant payloads and unmodeled mass distributions. Table 5 summarizes ISE, IAE, and ITAE values for both nominal and loaded conditions.

Figure 18. Comparison of low-level control signals under nominal and loaded conditions.

Figure 19. Comparison of trajectory-tracking performance: nominal vs. loaded conditions.

Figure 20. Tracking error metrics comparison for nominal and loaded experimental scenarios.

Table 5. Quantitative evaluation of tracking error for Experiment 4 (Load Disturbance Compensation).

Metric	Nominal	Loaded
Mean ISE	0.0243	0.0668
Mean IAE	0.5134	0.7656
Mean ITAE	6.3483	9.3463

5.5. Experiment 5: MPC vs. FMPC (Computational Cost)

The practical feasibility of the proposed hierarchical control scheme is central to its application in real-world environments. The architecture aims to achieve precise and robust trajectory tracking under strict constraints within a 10 ms sampling interval. This enables the potential deployment of the algorithm on embedded platforms with limited computational resources. To validate the optimization of the computational load, the proposed FMPC was benchmarked against a conventional, non-parameterized MPC. Both controllers utilized the same double-integrator model, identical constraints, cost-function weighting matrices, and numerical solver. As shown in Figure 21 and Table 6, the FMPC significantly reduces computational overhead while simultaneously improving tracking precision, yielding lower error values across all performance indices (ISE, IAE, and ITAE). Regarding execution time (Figure 22), the conventional MPC operates near the sampling limit, with its variance ($\mu \pm 2\sigma$) frequently exceeding the 10 ms threshold. Conversely, the FMPC consistently solves the optimization problem well below the sampling period with negligible variance.

Figure 21. First figure of the calculation time comparison.

Table 6. MPC vs. FMPC metrics.

Metric	MPC	FMPC
Mean ISE	0.0836	0.0243
Mean IAE	0.9728	0.5134
Mean ITAE	12.1433	6.3483
Mean computation time	9.4647 ms	0.8652 ms

Figure 22. Second figure of the calculation time comparison.

These results highlight the superior real-time performance of the FMPC and its suitability for fully embedded implementation.

6. Empirical Stability Analysis via Cost-Function Evolution

To provide empirical evidence of the closed-loop stability properties of the proposed FMPC algorithm, the temporal evolution of the cost function $J\left(k\right)$ and its first-order increment $\Delta J\left(k\right)=J\left(k\right)-J(k-1)$ were evaluated along ten independent experimental runs as described in experiments 1, 2, and 4. At each sampling instant, $J\left(k\right)$ was computed according to the standard quadratic form (10) with $N=30$, $T_s=10$ ms, and the steady-state input $u_d$ obtained from the standard rank condition associated with the augmented system matrix, in agreement with the classical reference-tracking formulation. The evaluation was performed on measured states and applied control inputs, so that the resulting trajectory of $J\left(k\right)$ reflects the actual closed-loop behavior of the plant rather than the predicted optimum of the underlying quadratic program.

The upper subplot of the Figure 23 reveals three distinct phases. During the initial transient ($t\in [0,2]$ s), the cost decreases by nearly two orders of magnitude, dropping from $J\left(0\right)\approx 7000$ to a residual value below 150. The subsequent regime exhibits a bounded steady-state behavior in which $J\left(k\right)$ remains confined within a small neighborhood of zero, punctuated by mild periodic excursions of approximately 250 units occurring at $t\approx 5$, 10, and 15 s. These excursions are consistent with the geometry of the reference: the lemniscate exhibits regions of maximal curvature at its central crossing and outer apices, where the tracking task becomes most demanding due to abrupt changes in the desired velocity vector. The periodicity, magnitude, and phase of these features are reproducible across the ten experimental runs, as evidenced by the narrow $\pm 2\sigma$ confidence bands surrounding the mean trajectory.

Figure 23. Cost function evolution.

The increment $\Delta J\left(k\right)$, displayed in the lower subplot, provides the more discriminating diagnostic. A short-lived positive lobe is observed during the first ∼0.4 s of operation, attributable to the physical inertia of the actuators; as the reference begins to evolve from rest, the cart cannot instantaneously match the prescribed velocity profile, causing the tracking error to grow over a small number of samples before the controller asserts effective authority. This phenomenon is intrinsic to any sampled-data implementation operating on real hardware and does not constitute a violation of the Lyapunov-type descent property of MPC, which is rigorously established for the value function $V\left(x\left(k\right)\right)=J^{★}\left(x\left(k\right)\right)$ associated with the optimal control sequence rather than for the realized cost computed from measured signals. Once the actuators reach an operating regime compatible with the reference dynamics, $\Delta J\left(k\right)$ becomes strongly negative, attaining values of approximately $-110$ during the dominant decay phase. After convergence to the steady-state regime, $\Delta J\left(k\right)$ oscillates in a tight neighborhood of zero, with both positive and negative excursions remaining below 5% of the peak descent rate observed during the transient.

The aggregate behavior is fully consistent with the Lyapunov-based stability arguments that are standard in the predictive control literature. The cost function exhibits an overall monotonic decrease over the transient phase, ultimate boundedness in the steady-state phase, and a contraction ratio of $J\left(t_f\right)/J\left(0\right)\ll 1$ that quantifies the controller’s regulating action. The minor positive excursions of $\Delta J\left(k\right)$ in the steady-state regime are explained by two well-understood mechanisms: first, the reference is not an equilibrium of the system but a periodic, non-stationary trajectory, so the cost cannot decay to zero but only to a residual level dictated by the curvature-induced infeasibility of perfect tracking; second, the cost is evaluated on the realized closed-loop trajectory rather than on the optimal predicted one, introducing a non-negative gap whose fluctuation is consistent with observed measurement noise and unmodeled dynamics. Critically, the standard deviation across the ten runs remains an order of magnitude smaller than the mean throughout the entire experiment, confirming that the observed convergence behavior is a structural property of the closed loop rather than a fortunate artifact of any particular trial. Taken together, these observations constitute strong empirical evidence that the proposed FMPC algorithm enforces practical asymptotic stability of the tracking error and exhibits the descent behavior expected of a properly tuned predictive control law.

7. Conclusions

This work presents a hierarchical control architecture for high-precision trajectory tracking of a car-like mobile robot operating under significant disturbances originating from the normal and tangential forces present in the wheels of the vehicle. The proposed framework integrates a low-level nonlinear dynamic controller with a high-level frugal model predictive controller, enabling simultaneous disturbance rejection and accurate path tracking at a prescribed cruise velocity. Experimental results demonstrate that the integration of disturbance-compensating low-level control, predictive high-level planning, and sensor-fusion-based state estimation provides an effective solution for reliable autonomous navigation in challenging environments. Future work will focus on extending the framework to fully onboard sensing systems, adapting the predictive layer to accommodate varying terrain conditions, and exploring cooperative implementations in multi-vehicle robotic systems.

A significant limitation of the current control scheme is its reliance on a kinematic bicycle model, which fails to guarantee stability or feasibility in high-speed maneuvers. To address this, future research will incorporate the bicycle’s dynamic model into the MPC framework. This will allow the system to compensate for lateral forces and varying tire-road friction, ensuring stability at higher velocities. Additionally, we aim to investigate a path-following reformulation where the trajectory is parameterized in the spatial domain. By representing the prediction model as a chain of integrators or decoupled double integrators, the MPC can be solved as a quadratic programming (QP) problem, significantly enhancing real-time computational efficiency.