![](bg3.png)
gain during control from a dual-control perspective.
We propose that the autonomous vehicle follows
the preplanned path passively; see Figure 1; without
replanning or exploring until the sensors reach an un-
known region or detect an obstacle. Upon detection
of an obstacle, the lower-level controller explores the
possible for a shorter way to reach the goal, while
reducing the uncertainty about the obstacle. This is
achieved by integrating an exploration objective into
the moving horizon optimization task for control. If
exploration becomes too expensive or the vehicle be-
comes stuck, a fallback mechanism is used, return-
ing the autonomous vehicle to the safe path to reach
the final destination; see Figure 1. The remainder of
the paper is structured as follows. In Section 2, we
present the problem formulation. Section 3 outlines
the hierarchical planning and control scheme, includ-
ing the lower-level exploration controller. Section 4
introduces the fallback strategy. The effectiveness of
the our approach is demonstrated in Section 5, before
summarizing the findings in Section 6.
2 PROBLEM FORMULATION
We consider an autonomous vehicle equipped with
onboard sensors that moves in a partially known envi-
ronment with static obstacles; see Figure 1. The pos-
sibly nonlinear vehicle dynamics are given in discrete
time:
x
k+1
= f (x
k
,u
k
), (1)
where x ∈ R
n
x
and u ∈ R
n
u
represent the vehicle states
and control input, and f : R
n
x
× R
n
u
→ R
n
x
. The state
vector x
k
=
p
k
T
,···
T
contains the center of mass co-
ordinates p
k
∈ R
n
, orientation, and velocities, pitch,
etc. The autonomous vehicle should move from an
Initial position A to a Final position B. We assume
that a safe path from A to B, based on the offline avail-
able environment avoiding unknown regions is avail-
able, see Figure1.
2.1 Planning in the Sensor Field of View
We assume that the autonomous vehicle is equipped
with an onboard sensor that has a limited field of
view, Z
t
see Figure 1. For simplicity, we assume that
this field of view is shaped as an ellipsoidal segment,
which is common for many sensor systems such as
LIDAR, radar, and cameras. At each sampling time,
t, the sensors onboard capture new information which
is used for control or replanning of the path. Often,
a hierarchical control and planning scheme is used,
intertwining the planning and control problem. We
propose that, starting at point A, the preplanned safe
path based on the offline map data is used by a model
predictive path following controller (Matschek et al.,
2019) to ensure the vehicle follows the path until the
sensor’s field of view, Z
t
, encounters unknown re-
gions or an unknown obstacle. At that point, a low-
level exploratory controller is activated to explore the
unknown region, potentially finding a more optimal
path while considering the sensor field of view and
the available map information, see Figure 2. Note
that in principle, ‘global’ path planning could be per-
formed whenever new environmental information is
encountered to find a new optimal path. However, this
approach is computationally expensive and often can-
not be performed on the vehicle itself due to hardware
limitations. Therefore, we propose online or sensor-
based path planning when new information becomes
available, with the aim of locally planning a safe path
within the sensor’s limited field of view. Upon detect-
ing an obstacle O
i
, where i ∈ {1, . . . , N
o
} and N
o
is the
number of obstacles, the local planner devises a safe
path within the field of view.
Furthermore, not that we do not focus on uncer-
tainties in the system dynamics or external distur-
bances acting on the vehicle. To ensure safety in
these cases, one could perform an additional con-
straint backoff, that is, add a safety region around the
obstacle (Soliman et al., 2022) and/or use tube-based
predictive control techniques.
Performing path planning over the field of view
is still generally computationally challenging for au-
tonomous systems. Thus, we propose to use a hi-
erarchical approach, where the low-level controller
”hides” the systems’ nonlinearities and uncertainties
and performs the exploration, while the high-level
planner uses a linear system model and is formulated
as a mixed-integer optimization problem looking over
a prediction horizon N
p
covering the field of view
Soliman et al. (2022). The mixed-integer formulation
using the linear system model is formulates as:
min
u
p
,d
t+N
p
∑
k=t
∥p
p
k
− p
p
B
∥
∞
+ ∥u
p
k
∥
∞
(2a)
s.t. x
p
k+1
= Ax
p
k
+ Bu
p
k
, x
p
t
= ˆx(t), (2b)
E
i,t
p
p
k
≥ e
i,t
+ M
big
(1 − d
i,k
), (2c)
Z
t
p
p
k
≤ z
t
, (2d)
A
in
x
p
k
≤ b
in
, (2e)
N
e
∑
k=1
d
i,k
= 1 (2f)
Here, x
p
k
denotes the predicted states of the linear
model. ˆx(t) is the measured system state at time t
Moving Horizon Planning and Control for Autonomous Vehicles with Active Exploration and Fallback Strategies
361