Next Article in Journal
Integrating Machine Learning with Intelligent Control Systems for Flow Rate Forecasting in Oil Well Operations
Previous Article in Journal
Multi-Level Feature Extraction and Classification for Lane Changing Behavior Prediction and POD-Based Evaluation
Previous Article in Special Issue
Development of a Programmable System Used for the Preparation of a Mixture of Flammable/Explosive Gases
 
 
Font Type:
Arial Georgia Verdana
Font Size:
Aa Aa Aa
Line Spacing:
Column Width:
Background:
Article

Optimizing Unmanned Air–Ground Vehicle Maneuvers Using Nonlinear Model Predictive Control and Moving Horizon Estimation

by
Alessandra Elisa Sindi Morando
1,2,*,
Alessandro Bozzi
1,
Simone Graffione
1,
Roberto Sacile
1 and
Enrico Zero
1
1
Department of Informatics, Bioengineering, Robotics and Systems Engineering, University of Genova, 16100 Genova, Italy
2
Heudiasyc (Heuristics and Diagnosis of Complex Systems) CNRS Laboratory of the Université de Technologie de Compiègne, 60200 Compiègne, France
*
Author to whom correspondence should be addressed.
Automation 2024, 5(3), 324-342; https://doi.org/10.3390/automation5030020
Submission received: 11 June 2024 / Revised: 15 July 2024 / Accepted: 25 July 2024 / Published: 30 July 2024

Abstract

:
In this paper, Nonlinear Model Predictive Control (NMPC) and Nonlinear Moving Horizon Estimator (NMHE) are combined to control, in a distributed way, a heterogeneous fleet composed of a steering car and a quadcopter. In particular, the ground vehicle in the role of the leader communicates its one-step future position to the drone, which keeps the formation along the desired trajectory. Inequality constraints are introduced in a switching control fashion to the leader’s NMPC formulation to avoid obstacles. In the literature, few works using NMPC and NMHE deal with these two vehicles together. Moreover, the presented scheme can tackle noisy, partial, and missing measurements of the agents’ state. Results show that the ground car can avoid detected obstacles, keeping the tracking errors of both robots in the order of a few centimeters, thanks to trustworthy NMHE estimates and NMPC predictions.

1. Introduction

Multiple robot systems have become widespread, as they can perform hard and possibly dangerous tasks for humans while improving efficiency and productivity. Among the plethora of applications, there are industry, agriculture, construction, and emergency rescue [1].
Among the different kinds of collaboration, hybrid teams can tackle more complex tasks requiring different capabilities, notably air–ground cooperation. On the one hand, Unmanned Aerial Vehicles (UAVs) have long-range sensors on board, making them suitable for monitoring from the sky. On the other side, Unmanned Ground Vehicles (UGVs) have short-range sensors and high load and computational capacities. The main issues are motion control, formation control, and collision avoidance. Vinicius et al. [2] proposed a light path following a virtual structure-based controller for package delivery. As the UAV flies over it, the UGV has to follow a desired trajectory while avoiding obstacles.
A PI optimal control theory is presented in [3], and results show that the proposed scheme can overcome input and model disturbances and linearization biases. Again considering air–ground cooperation, ref. [4] presents a leader–follower architecture, where the quadcopter has to reach the desired waypoints and send its position to the unicycle, which uses these data as the desired position. In addition, the ground agent should avoid obstacles around it.
Classical control methods assume complete knowledge of the system state without data loss. When agents are not tracked, the control action cannot be computed, and the robots must halt. Nonlinear Model Predictive Control (NMPC) provides a solution to this issue. In the absence of measurements, NMPC can apply the optimal control trajectory computed in the previous step until updated data become available. The key idea is to use an internal nonlinear model to predict the system’s future response. Due to its simplicity, MPC has been widely used in various applications, with numerous studies experimentally validating its superiority over classical linear controllers. In [5], the authors compared the performance of PID, LQR and MPC in controlling a drone. Both simulations and real experiments with a Parrot mini-drone were carried out. It turns out that MPC outperforms other methods, ensuring robustness, stability, and small errors in terms of maximum pitch deviation. Al-Saoudi et al. [6] carried out a similar study comparing model-based controllers such as PID, fuzzy logic, and MPC with an ANFIS-based controller. From the MATLAB/SIMULINK R2022a tests, it is shown that the MPC is superior, as it can handle input constraints and has lower overshoot and response time compared to other techniques. Taking into account a separation clutch system in the slipping state, ref. [7] compares PI-PD and LQR methods with a scheme composed of multiple MPCs but again this last turns out to be the best in terms of performance. Always in the field of autonomous driving, the paper [8] studies and evaluates in terms of tracking error different methods (Pure Pursuite, Stanley, LQR and MPC) for the trajectory tracking mission of a steering car. In both tests, MPC provides better accuracy both for the eight shape and the circle path. While NMPC has been consolidated over the years for controlling UGVs, leading to embedded implementations [9,10], it has been applied to UAVs only recently. Indeed, with the advent of more powerful microprocessors and efficient nonlinear solvers, a real-time application of NMPC for small drones is now possible [11].
However, one of the primary limitations of NMPC is the assumption that the complete state is measurable. To overcome this, NMPC is often paired with nonlinear estimators, such as the Nonlinear Moving Horizon Estimator (NMHE) [12]. NMHE addresses the dual problem of NMPC by providing state estimates that incorporate constraints and nonlinearities without assuming a specific error distribution. This pairing allows NMPC to function effectively even when full state information is not directly measurable. These two complementary state-of-the-art tools, when combined, can accurately control and estimate complex underactuated systems. In [13], the authors propose an NMHE-NMPC scheme to address the path-following problem of generalized N-vehicles under noisy measurements and disturbances. To evaluate its performance, simulations and real-world experiments have been performed to show its accuracy and applicability to real-time scenarios. A combination of both techniques is presented in [14] to control a selective catalytic reduction process for reducing engine emissions. In the context of UAVs, ref. [15] proposes a new NMPC method for UAV navigation that can deal with dynamic obstacles by predicting their movement and using a fast solver to find safe trajectories in real time, while [16] presents a nonlinear fault-tolerant control system for a quadcopter. This approach combines NMPC and NMHE to maintain trajectory control even in the presence of rotor failures. In the worst-case scenario where all motors are damaged, the NMHE estimates allow the NMPC to effectively stabilize and accurately follow the reference trajectory.
This work addresses the UAV-UGV formation and path-tracking problem. In particular, a distributed leader–follower NMPC-NMHE scheme is defined. The main novelties and contributions of this paper are the following:
(a)
Although promising results have been obtained in various applications, NMPC and NMHE have not been used together for air–ground cooperation.
(b)
To the best of the authors’ knowledge, these two receiving horizon techniques have not been used to control a heterogeneous fleet consisting of a steering car and a quadcopter. In fact, most current work considers unicycles as ground vehicles.
(c)
It is worth noting that the proposed solution deals with partially noisy state measurements and is, in principle, robust to information loss.
The paper unfolds in three key sections. Section 2 outlines the methods and definitions devised to tackle the focal problem under examination. Moving forward, Section 3 consolidates the numerical simulation results. Ultimately, Section 4 encapsulates the conclusions, emphasizing future avenues for development.

2. Materials and Methods

The following sections will introduce the mathematical models employed in the problem formulation. Even if the UAV and the UGV present different dynamics, an NMPC and an NMHE have been developed for both agents. Moreover, it is assumed that the steering car communicates to the drone the one-step prediction of its state.

2.1. UAV and UGV Models

For describing the drone dynamics, the model defined in [17] is used.
x 1 ˙ = x 2 x 2 ˙ = u 1 sin ( θ 1 ) / M y 1 ˙ = y 2 y 2 ˙ = u 1 cos ( θ 1 ) sin ( ϕ 1 ) / M z 1 ˙ = z 2 z 2 ˙ = u 1 cos ( θ 1 ) cos ( ϕ 1 ) / M g θ 1 ˙ = θ 2 θ 2 ˙ = u 2 ϕ 1 ˙ = ϕ 2 ϕ 2 ˙ = u 3 ψ 1 ˙ = ψ 2 ψ 2 ˙ = u 4
The state variables are defined according to Figure 1. Hereinafter, let the state and control variables of the UAV at time t be denoted as:
x U A V = x 1 x 2 y 1 y 2 z 1 z 2 θ 1 θ 2 ϕ 1 ϕ 2 ψ 1 ψ 2 T
u U A V = u 1 u 2 u 3 u 4 T
and rewrite Equation (1) in a vectorial form
x ˙ U A V = f U A V ( x U A V , u U A V )
To study the steering car, the Ackermann model is used [18]. All the state quantities of the ground vehicle of the ground robot are depicted in Figure 2. The control variables are the steering angle w 1 ( t ) and the forward acceleration w 2 ( t ) . The evolution model is
x ˙ = v cos ( θ ) y ˙ = v sin ( θ ) θ ˙ = ( v / L ) tan ( w 1 ) v ˙ = w 2
The remainder of this paper will make use of the following notation:
x U G V = x y θ v T
u U G V = w 1 w 2 T
Thus, Equation (5) becomes
x ˙ U G V = f U G V ( x U G V , u U G V )

2.2. UAV NMPC

The drone’s NMPC aims to find the optimal control input to be applied by solving an optimization problem. Given the current estimated quadcopter’s state x ^ U A V ( k ) , the objective is to minimize the deviations from the reference r U A V ( k ) over a prediction horizon H p weighted by Q U A V . To predict the future behavior of the UAV, the discrete-time evolution f U A V is used, obtained by discretizing Equation (4) with sampling time T s and applying a trapezoidal rule:
min i = 1 H p x U A V ( k + i | k ) r U A V ( k ) Q U A V 2
s . t . x U A V ( k | k ) = x ^ U A V ( k )
x U A V ( k + i + 1 | k ) = f U A V ( x U A V ( k + i | k ) , u U A V ( k + i | k ) )
x U A V m i n x U A V ( k + i | k ) x U A V M A X
The decision variables are the state and the input trajectory, respectively x U A V ( k + i | k ) and u U A V ( k + i | k ) , over the time window. To reduce the computational effort, it is assumed that after H u steps, the control remains constant. Regarding the reference, the drone should track the future position of the leader over the prediction horizon. Here, the trajectory is approximated with the future pose at the next step k + 1 obtained by the car’s NMPC (see Section 2.5):
r U A V ( k ) = x ( k + 1 | k ) 0 y ( k + 1 | k ) 0 z 1 , R e f ( k ) 0 0 0 0 0 θ ( k + 1 | k ) 0 T
Constraint Equation (9d) represents the structural limitations inherent to the drone’s design.
Conceptually, it is important to verify that the controller is stable and provides a control law that does not diverge, regardless of the system’s objective. Although the proof of stability is not included in this work, readers interested in a generalization may refer to Theorem 3 of [19], which is largely similar to the proposed theorem in this work, with the exception of minor constraints that are reasonably assumed not to affect the system’s stability. This is supported by the results in Section 3, which empirically confirm the validity of the control architecture for the proposed scenarios.

2.3. MHE for the UAV

The defined NMPC assumes that all state variables are known. However, some quantities cannot be measured directly, necessitating the introduction of an estimator, such as the MHE, which can deal with the exact nonlinear model and state and control boundaries. Taking into account a set of past noisy measurements,
y ( k i ) i = 0 , , H e
and control inputs
u ( k i ) i = 0 , , H e
over an estimation horizon H e , the estimated state trajectory over the time window
x ^ ( k i ) i = 0 , , H e
is obtained by solving a constrained nonlinear optimization problem that penalizes the variations of the predicted measurement from the actual past values. Once the sequence of estimates is obtained, only the final value is retained, and the estimation horizon is shifted forward to repeat the computations at the next instant k + 1 . It is important to note that although the focus here is on nonmeasurable quantities, this technique can also be employed to estimate unknown parameters.
For a generic unmanned vehicle, the equations are
min i = 1 H e y ( k i ) h ( x ^ ( k i ) , u ( k i ) ) 𝒱 2
s . t . x ^ ( k i + 1 ) = f ( x ^ ( k i ) , u ( k i ) )
x U V m i n x ^ ( k i ) x U V M A X
where f and h are, respectively, the evolution and the measurement models. When considering a Gaussian measurement noise
ν ( t ) N ( 0 n y × 1 , Σ ν ) Σ ν = diag ( σ 1 2 σ n y 2 )
the weighting matrix 𝒱 is chosen as
𝒱 = Σ ν 1 = diag ( 1 / σ 1 1 / σ n y )
In the case of the quadcopter, x ^ ( k i ) and u ( k i ) are, respectively, the estimate of the state x U A V and the control u U A V at instant k i , i = 0 , , H e . It is reasonable to assume that a motion capture system can measure the pose of the robot with high accuracy. Although the Euler angles are defined in the body frame, a rotation matrix from the global reference frame can be defined. However, although this aspect is not addressed in the current context, measurement noise should be considered when developing an embedded system:
ν U A V ( t ) N ( 0 6 × 1 , Σ ν U A V )
Σ ν U A V = diag ( σ x 1 2 σ y 1 2 σ z 1 2 σ θ 1 2 σ ϕ 1 2 σ ψ 1 2 )
𝒱 U A V = diag ( 1 / σ x 1 1 / σ y 1 1 / σ z 1 1 / σ θ 1 1 / σ ϕ 1 1 / σ ψ 1 )
h U A V ( x ^ ( k i ) , u ( k i ) ) = C U A V x ^ ( k i )
C U A V R 6 × 12 s . t .
C U A V i , j ] = 1 if i = 1 , , 6 j = 2 i 1 0 otherwise
The discrete-time evolution model used in the UAV MHE formulation is obtained through the Runge–Kutta (RK4) method:
k 1 = f U A V ( x ^ U A V ( k i ) , u U A V ( k i ) )
k 2 = f U A V ( x ^ U A V ( k i ) + ( T e / 2 ) k 1 , u U A V ( k i ) )
k 3 = f U A V ( x ^ U A V ( k i ) + ( T e / 2 ) k 2 , u U A V ( k i ) )
k 4 = f U A V ( x ^ U A V ( k i ) + ( T e / 2 ) k 3 , u U A V ( k i ) )
x ^ U A V ( k i + 1 ) = x ^ U A V ( k i ) + + ( T e / 6 ) ( k 1 + 2 k 2 + 2 k 3 + k 4 )
with T e the estimation time step. Finally, the constraint Equation (14c) becomes
x U A V m i n x ^ ( k i ) x U A V M A X

2.4. MHE for the UGV

Considering the UGV, an MHE should be introduced to estimate the forward velocity, which cannot be measured directly by the motion capture system. In particular,
ν U G V ( t ) N ( 0 3 × 1 , Σ ν U G V )
Σ ν U G V = diag ( σ x 2 σ y 2 σ θ 2 )
𝒱 U G V = diag ( 1 / σ x 1 / σ y 1 / σ θ )
h U G V ( x ^ ( k i ) , u ( k i ) ) = C U G V x ^ ( k i )
C U G V = I 3 × 4
Similarly, the RK4 method has been employed for the discretization of state differential equations in the ground agent prediction model Equation (8).

2.5. UGV NMPC with Obstacle Avoidance

To control the steering car and avoid obstacles in the meantime, a NMPC is used. Assume that the UGV has remote sensing technology on board, such as LIDAR, with a 360 full scanning range up to d L I D A R meters. Once an obstacle is detected in the sensor’s field, the robot should keep a safe distance d S a f e from it as depicted in Figure 3.
The mathematical formulation of the steering car’s NMPC in the absence of obstacles is
min i = 1 H p x U G V ( k + i | k ) r U G V ( k + i ) Q U G V 2 + i = 0 H u 1 u U G V ( k + i | k ) R 2 + + i = 0 H u 1 Δ u U G V ( k + i | k ) R Δ 2
s . t . x U G V ( k | k ) = x ^ U G V ( k )
x U G V ( k + i + 1 | k ) = f U G V ( x U G V ( k + i | k ) , u U G V ( k + i | k ) )
0 e x y ( k + H p | k ) T e x y ( k + H p | k ) ( d S a f e + ε ) 2
x U G V m i n x U G V ( k + i | k ) x U G V M A X
u U G V m i n u U G V ( k + i | k ) u U G V M A X
where, for readability, the input variation is denoted as
Δ u U G V ( k + i + 1 | k ) = u U G V ( k + i + 1 | k ) u U G V ( k + i | k )
Δ u U G V ( k | k ) = u U G V ( k | k ) u U G V ( k 1 )
with u U G V ( k 1 ) the control input at instant k. Let us see in detail the formulation which is similar to the one in Section 2.2. In the objective function, the control input and its variations are also penalized respectively with weights R and R Δ . Secondly, the decision variables are the future evolution of the state
x U G V ( k + i | k ) i = 0 , , H p
and the control trajectory
u U G V ( k + i | k ) i = 0 , , H u 1
For the ground vehicle, a reference trajectory (which varies along the prediction horizon) is defined for all state variables
r U G V ( k + i ) = x R e f ( k + i ) y R e f ( k + i ) θ R e f ( k + i ) v R e f ( k + i ) T
Concerning the constraints, x ^ U G V ( k ) is the MHE estimate of the ground vehicle state. In Equation (25c), f U G V represents the evolution model used in the UGV estimator. Terminal region constraints Equation (25d) have been added to force the vehicle to stay in a neighborhood of the reference at the end of the prediction horizon. In Equation (25d), e x y ( k + H p | k ) is the ( x , y ) -tracking error at instant k + H p :
e x y ( k + H p | k ) = x ( k + H p | k ) r x ( k + H p ) y ( k + H p | k ) r y ( k + H p )
Note that the ( x , y ) -terminal region has been chosen as a circular area of size d S a f e + ε , as, in theory, the reference could coincide with the obstacle as in the case study considered in Section 3. Bounds are defined for the state and the control.
Up to now, Equation (25) has considered the scenario in which no obstruction is found by the onboard sensor. The approach to address obstacle avoidance closely mirrors the one of Sani et al. [20]. If a barrier is encountered, the controller is switched, and a further family of constraints is added to Equation (25):
d O b s ( k + i | k ) T d O b s ( k + i | k ) ( d S a f e + ε ) 2
with
d O b s ( k + i | k ) = x ( k + i | k ) x O b s ( k ) y ( k + i | k ) y O b s ( k )
and ( x O b s , y O b s ) is the position of the detected obstacle. The margin ε > 0 is added such that the safety distance is always respected.

3. Results

To validate the proposed solution, various simulations were conducted in MATLAB/Simulink, altering the reference trajectories, the number of obstacles, and their positions. In particular, to implement MHE and the switching NMPC of the UGV, the CasADi framework is used as in [10,20]. CasADi [21] is an open-source tool that provides different facilities useful in academic and industrial applications, notably the Internal Point Optimizer to solve nonlinear programming problems efficiently.
The robots parameters are set to L = 0.14 m and M = 0.50 k g in accordance with the values of the JetRacer race car and the Parrot Bebop 2.0 drone. It is assumed that the agent can detect barriers up to d L I D A R = 5 m , in line with the datasheet of the RPLIDAR A1 included with the JetRacer ROS Kit. The safety distance for the UGV is set to d S a f e = 0.25 m and ε = 0.10 m . At the beginning of the simulations, the states of both robots are
x U G V ( 0 ) = 2 m 0 m π / 2 rad 0 m   s 1 T
x U A V ( 0 ) = 2 m 0 m   s 1 0 m 0 m   s 1 1 m 0 m   s 1              0 rad 0 rad s 1 0 rad 0 rad s 1 π / 2 rad 0 rad s 1 T
Concerning the structural boundaries of the quadcopter, the maximum tilt angle, vertical speed, and rotational speed around Z B are
θ 1 ( t ) 30 π / 180 rad
ϕ 1 ( t ) 30 π / 180 rad
z 1 ( t ) 1 m   s 1
ψ 2 ( t ) 100 π / 180 rad s 1
On the other hand, for the steering car’s state, only the forward velocity is bounded
v ( t ) 0.30 m   s 1
while for its control inputs
0.30 rad w 1 ( t ) 0.80 rad
w 2 ( t ) 1 m s 2
Regarding the NMPC parameters, T s = 0.1 s is set for both H p = 10 and H u = 5 , while for the weights
Q U G V = 1000 diag ( 20 20 1 1 )
Q U A V = 100 diag ( 1 0 1 0 1 0 0 0 0 0 1 0 )
R = 10 I 2 × 2 R Δ = 100 I 2 × 2
Concerning the estimators, H e = 5 , T e = 0.1 s is chosen, and
σ x 1 = σ y 1 = σ z 1 = σ x = σ y = 0.02   ×   10 3 m σ θ 1 = σ ϕ 1 = σ ψ 1 = σ θ = 0.1 rad
as defined in the Optitrack’s specification.
For all the cases studies, the quantitative analysis is split considering the robots separately, as, indeed, the controller is distributed. Let us focus first on the UGV. To investigate the NMPC performance, the Root Mean Square Error (RMSE) is considered for the tracking:
R M S E r U G V = E x U G V ( k ) r U G V ( k ) 2
and for the one-step prediction
R M S E y + 1 = E y ( k + 1 | k ) x U G V ( k + 1 ) 2
Regarding the MHE, the estimation’s RMSE is considered as
R M S E x ^ U G V = E x ^ U G V ( k ) x U G V ( k ) 2
Regarding the quadcopter tracking, it is important to distinguish the deviation from the reference used by the NMPC objective, where zeros corresponding to nontracked quantities are not considered:
r U A V ( k ) = x ( k + 1 | k ) y ( k + 1 | k ) z 1 , R e f ( k ) θ ( k + 1 | k ) T
and the actual one
r T , U A V ( k ) = x R e f ( k ) y R e f ( k ) z 1 , R e f ( k ) θ R e f T
Depending on whether Equation (43) or Equation (44) is chosen, two tracking errors can be defined:
R M S E r U A V = E x T , U A V ( k ) r U A V ( k ) 2
R M S E r T , U A V = E x T , U A V ( k ) r T , U A V ( k ) 2
where x T , U A V ( k ) is the subset of the tracked drone’s state’s components, i.e.,
x T , U A V ( k ) = x 1 ( k ) y 1 ( k ) z 1 ( k ) ψ 1 ( k ) T
Finally, to evaluate the MHE performance, the metric below is considered
R M S E x ^ U A V = E x ^ U A V ( k ) x U A V ( k ) 2

3.1. Circle with One Obstacle

The first scenario lasts 2 min and considers a circular trajectory centered in the origin with radius of 2 m , described by the following equations:
x R e f ( t ) = 2 cos 2 π 60 t y R e f ( t ) = 2 sin 2 π 60 t θ R e f ( t ) = 2 π 60 t + π 2 v R e f ( t ) = 2 2 π 60
A static obstacle is placed along the reference trajectory in ( x O b s , y O b s ) = ( 2 m , 0 m ) . While the UGV should track the reference, the drone should follow the leader, keeping an altitude defined as
z 1 , R e f ( t ) = 2 m if t < 1 min 3 m if t 1 min
The aim is to demonstrate that even if the drone must increase its altitude to avoid an obstacle, the controller effectively manages this change in trajectory. It should be noted that the UAV NMPC formulation currently does not include constraints such as Equation (31). Figure 4 shows the trajectories of the two agents.
The UGV moves in a circle as long as it maintains a safe distance from the obstacle. When the car approaches the obstacle, it turns away to avoid it and then realigns with the reference trajectory as illustrated in Figure 5. Meanwhile, the UAV keeps the formation with the leader and increases its height when required.
Table 1 summarizes all the indexes. The small errors in estimating and predicting the vehicle’s state allow to track the reference with deviations of a few centimeters and radiant.
As summed up in Table 2, resorting to the one-step prediction ensures the tracking of the desired attitude. Naturally, Equation (45) is smaller in magnitude than Equation (46), as in the NMPC formulation there appears r U A V ( k ) and not r T , U A V ( k ) . Still, the variations from the actual reference are quite satisfactory.
Table 3 reports for each quadcopter’s state’s components the negligible estimation error.

3.2. Circle with Two Obstacles

The second case study maintains the circular trajectory previously described but considers two obstacles, placed in ( x O b s 1 , y O b s 1 ) = ( 0 m , 2 m ) and ( x O b s 2 , y O b s 2 ) = ( 0 m , 2 m ) . The aircraft should adhere to a specified take-off reference trajectory as delineated below:
z 1 , R e f ( t ) = ( 1 + ( t / 30 ) ) m if t < 30 s 2 m if t 30 s
The simulation time is also reduced to one minute. Figure 6 and Figure 7 demonstrate that both the leader and the follower qualitatively follow the reference without collisions. Table 4, Table 5 and Table 6 show the performance indices for both agents. In order to avoid the two obstacles, the ground robot has to deviate its trajectory, which leads to an increase in its R M S E x of Equation (40). The drone accurately follows the one-step prediction of the car. However, the increase in the leader’s tracking error caused by the obstacles is reflected in the follower’s performance.

3.3. Lemniscate of Bernoulli with One Obstacle

For the third simulation, Bernoulli’s lemniscate is chosen as the desired trajectory, described by the following equations:
x R e f ( t ) = 2 c ( t ) cos t 2 π 60 y R e f ( t ) = c ( t ) sin 2 t 2 π 60 θ R e f ( t ) = arctan 2 ( v y , R e f ( t ) , v x , R e f ( t ) ) v R e f ( t ) = v x , R e f ( t ) 2 + v y , R e f ( t ) 2
with
c ( t ) = 2 / 3 cos 2 t 2 π 60
v x , R e f ( t ) = d x R e f ( τ ) d τ τ = t v y , R e f ( t ) = d y R e f ( τ ) d τ τ = t
and the only obstacle is in the origin. The reference altitude of the UAV is kept constant at z 1 , R e f ( t ) = 2 m .
The trajectories of the two agents are depicted in Figure 8 and Figure 9. The UGV initially follows the lemniscate path until it approaches too close to the origin. At that point, it navigates around the obstacle, resulting in a decrease in tracking accuracy as shown in Table 7. Due to the control constraints, the agent gradually returns to the reference path without sudden movements. When the steering car returns to its starting point, it must pass the origin again, thereby repeating the same behavior. Regarding the quadcopter, except for the initial moments of the simulation due to the initial conditions, it maintains the desired height while following the leader from above. For a quantitative analysis, see Table 8 and Table 9.

3.4. Lemniscate of Bernoulli with Two Obstacles

The last case study is similar to the previous one, but a second obstacle is added. Both are in the reference positions that the robot should ideally pass at t = 24 s and t = 54 s . To complicate the test, the constant height is replaced by a decreasing trajectory:
z 1 , R e f ( t ) = 2 m if t < 30 s 2 ( 1.8 ( t 30 ) / 30 ) m if t 30 s
The resulting performance indices are presented in Table 10, Table 11 and Table 12. As can be seen from Figure 10 and Figure 11, considerations similar to those for the single barrier case can be drawn. Note that the RSMEx and RMSEy of the tracking error Equation (40) are smaller compared to the values in the first row of Table 10, which remain the worst values among all the simulations. Nevertheless, in none of the case studies did the errors diverge and the ground vehicle entered the unsafe region.

4. Discussion

In this paper, nonlinear rolling horizon techniques are applied to a heterogeneous fleet. In particular, a distributed solution is proposed to address the formation path-tracking problem while avoiding obstacles. Results across various scenarios and maneuvers confirm the validity of the proposed control architecture, ensuring small errors with respect to reference trajectories. The possibilities for future research are manifold. Firstly, the results suggest that the proposed scheme ensures small tracking errors, but still the stability and convergence of the NMPC should be proved in a more rigorous way. Secondly, the works [5,6,7,8] indicate the superiority of NMPC over classical controllers. Conducting a similar comparative analysis, particularly in scenarios involving data loss, would be valuable to quantify the improvements in performance brought by the usage of a NMPC. Lastly, to go beyond numerical simulation, the implementation and embedding of the control architecture in real robots, such as the Parrot Bebop 2.0 and JetRacer equipped with LIDAR mapping, would strengthen the validation and applicability of the proposed methods in practical scenarios.

Author Contributions

Conceptualization, A.E.S.M., A.B., S.G., R.S. and E.Z.; methodology, A.E.S.M., A.B., S.G., R.S. and E.Z.; software, A.E.S.M., A.B., S.G., R.S. and E.Z.; validation, A.E.S.M., A.B., S.G., R.S. and E.Z.; formal analysis, A.E.S.M., A.B., S.G., R.S. and E.Z.; investigation, A.E.S.M., A.B., S.G., R.S. and E.Z.; resources, A.E.S.M., A.B., S.G., R.S. and E.Z.; data curation, A.E.S.M., A.B., S.G., R.S. and E.Z.; writing—original draft preparation, A.E.S.M., A.B., S.G., R.S. and E.Z.; writing—review and editing, A.E.S.M., A.B., S.G., R.S. and E.Z.; visualization, A.E.S.M., A.B., S.G., R.S. and E.Z.; supervision, A.E.S.M., A.B., S.G., R.S. and E.Z.; project administration, A.E.S.M., A.B., S.G., R.S. and E.Z.; funding acquisition, A.E.S.M., A.B., S.G., R.S. and E.Z. All authors have read and agreed to the published version of the manuscript.

Funding

This research received no external funding.

Institutional Review Board Statement

Not applicable.

Informed Consent Statement

Not applicable.

Data Availability Statement

Conflicts of Interest

The authors declare no conflicts of interest.

References

  1. Verma, J.K.; Ranga, V. Multi-Robot Coordination Analysis, Taxonomy, Challenges and Future Scope. J. Intell. Robot. Syst. 2021, 102, 10. [Google Scholar] [CrossRef] [PubMed]
  2. Bacheti, V.P.; Brandão, A.S.; Sarcinelli-Filho, M. A Path-Following Controller for a UAV-UGV Formation Performing the Final Step of Last-Mile-Delivery. IEEE Access 2021, 9, 142218–142231. [Google Scholar] [CrossRef]
  3. Zhang, J.; Yue, X.; Zhang, H.; Xiao, T. Optimal Unmanned Ground Vehicle—Unmanned Aerial Vehicle Formation-Maintenance Control for Air-Ground Cooperation. Appl. Sci. 2022, 12, 3598. [Google Scholar] [CrossRef]
  4. Tassanbi, A.; Iskakov, A.; Do, T.D.; Ali, M.H. Interactive Real-time Leader Follower Control System for UAV and UGV. In Proceedings of the 2022 International Conference on Electrical, Computer, Communications and Mechatronics Engineering (ICECCME), Male, Malediven, 16–18 November 2022; pp. 1–8. [Google Scholar] [CrossRef]
  5. Okasha, M.; Kralev, J.K.; Islam, M. Design and Experimental Comparison of PID, LQR and MPC Stabilizing Controllers for Parrot Mambo Mini-Drone. Aerospace 2022, 9, 298. [Google Scholar] [CrossRef]
  6. Al-Saoudi, A.F.; Al-Aubidy, K.M.; Al-Mahasneh, A.J. Comparison of PID, Fuzzy Logic, ANFIS and Model Predictive Controllers for Cruise Control System. In Proceedings of the 2024 21st International Multi-Conference on Systems, Signals & Devices (SSD), Erbil, Iraq, 22–25 April 2024; pp. 263–265. [Google Scholar]
  7. Kaçmaz, B.; Söylemez, M.T. Implementation and Comparison of PID, PI-PD, LQR and MPC on Separation Clutch System in Slip. In Proceedings of the 2021 22nd IEEE International Conference on Industrial Technology (ICIT), Valencia, Spain, 10–12 March 2021; Volume 1, pp. 61–67. [Google Scholar]
  8. Liu, J.; Yang, Z.; Huang, Z.; Li, W.; Dang, S.; Li, H. Simulation Performance Evaluation of Pure Pursuit, Stanley, LQR, MPC Controller for Autonomous Vehicles. In Proceedings of the 2021 IEEE International Conference on Real-Time Computing and Robotics (RCAR), Xining, China, 15–19 July 2021; pp. 1444–1449. [Google Scholar]
  9. Cenerini, J.; Mehrez, M.W.; Han, J.w.; Jeon, S.; Melek, W. Model Predictive Path Following Control without terminal constraints for holonomic mobile robots. Control Eng. Pract. 2023, 132, 105406. [Google Scholar] [CrossRef]
  10. Bozzi, A.; Graffione, S.; Kockelkoren, M.M.; Sacile, R.; Zero, E. Path Tracking for Wheeled Mobile Robot Using Non Linear Model Predictive Control in Indoor Environment. In Proceedings of the 2023 9th International Conference on Control, Decision and Information Technologies (CoDIT), Rome, Italy, 3–6 July 2023; pp. 1379–1384. [Google Scholar] [CrossRef]
  11. Recalde, L.F.; Guevara, B.S.; Andaluz, V.; Varela, J.; Gimenez, J.; Gandolfo, D. Nonlinear MPC for Multiple Quadrotors in Dynamic Environments. In Proceedings of the 2022 International Conference on Unmanned Aircraft Systems (ICUAS), Dubrovnik, Croatia, 21–24 June 2022; pp. 1201–1209. [Google Scholar] [CrossRef]
  12. Biegler, L. A perspective on nonlinear model predictive control. Korean J. Chem. Eng. 2021, 38, 1317–1332. [Google Scholar] [CrossRef]
  13. Deniz, N.; Jorquera, F.; Torres-Torriti, M.; Cheein, F.A. Model predictive path-following controller for Generalised N-Trailer vehicles with noisy sensors and disturbances. Control Eng. Pract. 2024, 142, 105747. [Google Scholar] [CrossRef]
  14. Aaltonen, O. Nonlinear Model Predictive Control and Estimation applied to Selective Catalytic Reduction. Master’s Thesis, Åbo Akademi University, Turku, Finland, 2022. [Google Scholar]
  15. Lindqvist, B.; Mansouri, S.S.; Agha-mohammadi, A.A.; Nikolakopoulos, G. Nonlinear MPC for collision avoidance and control of UAVs with dynamic obstacles. IEEE Robot. Autom. Lett. 2020, 5, 6001–6008. [Google Scholar] [CrossRef]
  16. Eltrabyly, A.; Ichalal, D.; Mammar, S. Quadcopter Trajectory Tracking in the Presence of 4 Faulty Actuators: A Nonlinear MHE and MPC Approach. IEEE Control Syst. Lett. 2022, 6, 2024–2029. [Google Scholar] [CrossRef]
  17. Castillo Garcia, P.; Munoz, L.; Gil, P. Modeling Approaches—The results in this chapter were developed in collaboration with H. Abaunza and J. Carino from the UMI LAFMIA 3175, Mexico. In Indoor Navigation Strategies for Aerial Autonomous Systems; Elsevier: Amsterdam, The Netherlands, 2017; pp. 31–50. [Google Scholar] [CrossRef]
  18. Lin, X.; Chen, X. Realization of Ackermann robot obstacle avoidance navigation based on Multi-sensor fusion SLAM. In Proceedings of the 2023 5th International Conference on Electronics and Communication, Network and Computer Technology (ECNCT), Guangzhou, China, 18–20 August 2023; pp. 354–359. [Google Scholar] [CrossRef]
  19. Köhler, J.; Müller, M.A.; Allgöwer, F. A nonlinear tracking model predictive control scheme for dynamic target signals. Automatica 2020, 118, 109030. [Google Scholar] [CrossRef]
  20. Sani, M.; Robu, B.; Hably, A. Dynamic Obstacles Avoidance Using Nonlinear Model Predictive Control. In Proceedings of the IECON 2021—47th Annual Conference of the IEEE Industrial Electronics Society, Toronto, ON, Canada, 13–16 October 2021; pp. 1–6. [Google Scholar] [CrossRef]
  21. Andersson, J.A.E.; Gillis, J.; Horn, G.; Rawlings, J.B.; Diehl, M. CasADi—A software framework for nonlinear optimization and optimal control. Math. Program. Comput. 2019, 11, 1–36. [Google Scholar] [CrossRef]
Figure 1. The quadcopter state consists of its position, translational velocity (both in the global reference frame), orientation, and rotational velocities (expressed in terms of Euler angles and in the body frame). The dynamics are obtained using the Euler–Lagrange approach.
Figure 1. The quadcopter state consists of its position, translational velocity (both in the global reference frame), orientation, and rotational velocities (expressed in terms of Euler angles and in the body frame). The dynamics are obtained using the Euler–Lagrange approach.
Automation 05 00020 g001
Figure 2. UGV Ackermann’s dynamic model with corresponding state and control variables.
Figure 2. UGV Ackermann’s dynamic model with corresponding state and control variables.
Automation 05 00020 g002
Figure 3. Obstacles are detected up to a distance d L I D A R , and the UGV should keep a safe distance d S a f e .
Figure 3. Obstacles are detected up to a distance d L I D A R , and the UGV should keep a safe distance d S a f e .
Automation 05 00020 g003
Figure 4. Trajectories of the two robots versus the reference. In grey, the unsafe obstacle’s neighborhood.
Figure 4. Trajectories of the two robots versus the reference. In grey, the unsafe obstacle’s neighborhood.
Automation 05 00020 g004
Figure 5. Top view of the trajectory of the UVs. Although the reference passes close to the obstacle, the steering car deviates to keep the safe distance.
Figure 5. Top view of the trajectory of the UVs. Although the reference passes close to the obstacle, the steering car deviates to keep the safe distance.
Automation 05 00020 g005
Figure 6. Trajectories of the two robots versus the reference. The drone increases, in the first half of the simulation, its altitude up to 2 m .
Figure 6. Trajectories of the two robots versus the reference. The drone increases, in the first half of the simulation, its altitude up to 2 m .
Automation 05 00020 g006
Figure 7. Top view of the trajectory of the UVs in the second scenario with two obstacles along the reference trajectory.
Figure 7. Top view of the trajectory of the UVs in the second scenario with two obstacles along the reference trajectory.
Automation 05 00020 g007
Figure 8. Trajectories of the two robots versus the reference. The drone’s reference altitude is 2 m.
Figure 8. Trajectories of the two robots versus the reference. The drone’s reference altitude is 2 m.
Automation 05 00020 g008
Figure 9. Top view of the trajectory of the UVs in the third scenario with the reference trajectory of the lemniscate of Bernoulli.
Figure 9. Top view of the trajectory of the UVs in the third scenario with the reference trajectory of the lemniscate of Bernoulli.
Automation 05 00020 g009
Figure 10. Trajectories of the ground and the aerial robots. The UGV has to follow the lemniscate of Bernoulli trajectory while avoiding two obstacles on its way. The quadcopter in the second half of the simulation reduces its height.
Figure 10. Trajectories of the ground and the aerial robots. The UGV has to follow the lemniscate of Bernoulli trajectory while avoiding two obstacles on its way. The quadcopter in the second half of the simulation reduces its height.
Automation 05 00020 g010
Figure 11. Top view of the two agents’ paths compared with the reference in the fourth scenario.
Figure 11. Top view of the two agents’ paths compared with the reference in the fourth scenario.
Automation 05 00020 g011
Table 1. The UGV MHE-NMPC scheme performance—Scenario n.1.
Table 1. The UGV MHE-NMPC scheme performance—Scenario n.1.
RMSE x [m] RMSE y [m] RMSE θ [rad] RMSE v [m s−1]
Equation (40)5.30 × 10−21.60 × 10−11.53 × 10−15.77 × 10−2
Equation (41)1.55 × 10−21.576 × 10−22.39 × 10−21.06 × 10−2
Equation (42)3.24 × 10−54.46 × 10−56.49 × 10−41.73 × 10−4
Table 2. The UAV NMPC performance—Scenario n.1.
Table 2. The UAV NMPC performance—Scenario n.1.
RMSE x 1 [m] RMSE y 1 [m] RMSE z 1 [m] RMSE ψ 1 [rad]
Equation (45)8.30 × 10−28.74 × 10−28.94 × 10−26.54 × 10−2
Equation (46)8.07 × 10−22.00 × 10−18.94 × 10−21.52 × 10−1
Table 3. The UAV MHE performance for all components (★)—Scenario n.1.
Table 3. The UAV MHE performance for all components (★)—Scenario n.1.
RMSE Est , UAV Unit
x 1 1.05 × 10−3 m ]
x 2 2.41 × 10−2 m   s 1 ]
y 1 1.01 × 10−3 m ]
y 2 2.27 × 10−2 m   s 1 ]
z 1 2.59 × 10−3 m ]
z 2 1.40 × 10−3 m   s 1 ]
θ 1 1.63 × 10−2 rad ]
θ 2 5.25 × 10−2 rad s 1 ]
ϕ 1 1.59 × 10−2 m ]
ϕ 2 5.045 × 10−2 m   s 1 ]
ψ 1 1.28 × 10−3 m ]
ψ 2 4.18 × 10−3 m   s 1 ]
Table 4. The UGV MHE-NMPC scheme performance—Scenario n.2.
Table 4. The UGV MHE-NMPC scheme performance—Scenario n.2.
RMSE x [m] RMSE y [m] RMSE θ [rad] RMSE v [m s−1]
Equation (40)2.32 × 10−17.44 × 10−22.17 × 10−18.21 × 10−2
Equation (41)1.66 × 10−21.61 × 10−23.20 × 10−21.50 × 10−2
Equation (42) 5.96 × 10−5 3.62 × 10−5 6.55 × 10−4 2.30 × 10−4
Table 5. The UAV NMPC performance—Scenario n.2.
Table 5. The UAV NMPC performance—Scenario n.2.
RMSE x 1 [m] RMSE y 1 [m] RMSE z 1 [m] RMSE ψ 1 [rad]
Equation (45)9.05 × 10−28.28 × 10−27.99 × 10−38.58 × 10−2
Equation (46) 2.80 × 10−1 9.08 × 10−2 7.99 × 10−3 2.14 × 10−1
Table 6. The UAV MHE performance for all components (★)—Scenario n.2.
Table 6. The UAV MHE performance for all components (★)—Scenario n.2.
RMSE Est , UAV Unit
x 1 4.49 × 10−5 m ]
x 2 1.15 × 10−3 m   s 1 ]
y 1 6.87 × 10−5 m ]
y 2 1.22 × 10−3 m   s 1 ]
z 1 1.48 × 10−5 m ]
z 2 4.93 × 10−5 m   s 1 ]
θ 1 1.03 × 10−3 rad ]
θ 2 3.88 × 10−3 rad s 1 ]
ϕ 1 1.03 × 10−3 m ]
ϕ 2 3.88 × 10−2 m   s 1 ]
ψ 1 1.29 × 10−3 m ]
ψ 2 4.19 × 10−3 m   s 1 ]
Table 7. The UGV MHE-NMPC scheme performance—Scenario n.3.
Table 7. The UGV MHE-NMPC scheme performance—Scenario n.3.
RMSE x [m] RMSE y [m] RMSE θ [rad] RMSE v [m s−1]
Equation (40)3.26 × 10−11.93 × 10−12.84 × 10−19.55 × 10−2
Equation (41)1.79 × 10−21.62 × 10−23.39 × 10−21.53 × 10−2
Equation (42)1.17 × 10−46.36 × 10−57.02 × 10−45.13 × 10−4
Table 8. The UAV NMPC performance—Scenario n.3.
Table 8. The UAV NMPC performance—Scenario n.3.
RMSE x 1 [m] RMSE y 1 [m] RMSE z 1 [m] RMSE ψ 1 [rad]
Equation (45)9.16 × 10−27.01 × 10−28.46 × 10−29.53 × 10−2
Equation (46)3.63 × 10−12.16 × 10−18.46 × 10−22.90 × 10−1
Table 9. The UAV MHE performances for all components (★)—Scenario n.3.
Table 9. The UAV MHE performances for all components (★)—Scenario n.3.
RMSE Est , UAV Unit
x 1 8.14 × 10−4 m ]
x 2 1.84 × 10−2 m   s 1 ]
y 1 1.25 × 10−3 m ]
y 2 2.82 × 10−2 m   s 1 ]
z 1 2.57 × 10−3 m ]
z 2 1.13 × 10−3 m   s 1 ]
θ 1 1.23 × 10−2 rad ]
θ 2 3.92 × 10−2 rad s 1 ]
ϕ 1 1.97 × 10−2 m ]
ϕ 2 6.21 × 10−2 m   s 1 ]
ψ 1 1.29 × 10−3 m ]
ψ 2 4.21 × 10−3 m   s 1 ]
Table 10. The UGV MHE-NMPC scheme performance—Scenario n.4.
Table 10. The UGV MHE-NMPC scheme performance—Scenario n.4.
RMSE x [m] RMSE y [m] RMSE θ [rad] RMSE v [m s−1]
Equation (40)2.17 × 10−14.61 × 10−22.03 × 10−17.65 × 10−2
Equation (41)1.50 × 10−21.92 × 10−23.46 × 10−21.51 × 10−2
Equation (42) 2.91 × 10−5 2.85 × 10−5 6.94 × 10−4 9.77 × 10−5
Table 11. The UAV NMPC performance—Scenario n.4.
Table 11. The UAV NMPC performance—Scenario n.4.
RMSE x 1 [m] RMSE y 1 [m] RMSE z 1 [m] RMSE ψ 1 [rad]
Equation (45)8.22 × 10−26.10 × 10−21.12 × 10−19.20 × 10−2
Equation (46) 2.54 × 10−1 6.30 × 10−2 1.13 × 10−12.18 × 10−1
Table 12. The UAV MHE performance for all components (★)—Scenario n.4.
Table 12. The UAV MHE performance for all components (★)—Scenario n.4.
RMSE Est , UAV Unit
x 1 8.14 × 10−4 m ]
x 2 1.84 × 10−2 m   s 1 ]
y 1 1.25 × 10−3 m ]
y 2 2.82 × 10−2 m   s 1 ]
z 1 2.57 × 10−3 m ]
z 2 1.13 × 10−3 m   s 1 ]
θ 1 1.22 × 10−2 rad ]
θ 2 3.92 × 10−2 rad s 1 ]
ϕ 1 1.97 × 10−2 m ]
ϕ 2 6.21 × 10−2 m   s 1 ]
ψ 1 1.29 × 10−3 m ]
ψ 2 4.21 × 10−3 m   s 1 ]
Disclaimer/Publisher’s Note: The statements, opinions and data contained in all publications are solely those of the individual author(s) and contributor(s) and not of MDPI and/or the editor(s). MDPI and/or the editor(s) disclaim responsibility for any injury to people or property resulting from any ideas, methods, instructions or products referred to in the content.

Share and Cite

MDPI and ACS Style

Morando, A.E.S.; Bozzi, A.; Graffione, S.; Sacile, R.; Zero, E. Optimizing Unmanned Air–Ground Vehicle Maneuvers Using Nonlinear Model Predictive Control and Moving Horizon Estimation. Automation 2024, 5, 324-342. https://doi.org/10.3390/automation5030020

AMA Style

Morando AES, Bozzi A, Graffione S, Sacile R, Zero E. Optimizing Unmanned Air–Ground Vehicle Maneuvers Using Nonlinear Model Predictive Control and Moving Horizon Estimation. Automation. 2024; 5(3):324-342. https://doi.org/10.3390/automation5030020

Chicago/Turabian Style

Morando, Alessandra Elisa Sindi, Alessandro Bozzi, Simone Graffione, Roberto Sacile, and Enrico Zero. 2024. "Optimizing Unmanned Air–Ground Vehicle Maneuvers Using Nonlinear Model Predictive Control and Moving Horizon Estimation" Automation 5, no. 3: 324-342. https://doi.org/10.3390/automation5030020

Article Metrics

Back to TopTop