Navigating Discrete Difference Equation Governed WMR
by Virtual Linear Leader Guided HMPC
Chao Huang, Xin Chen, Enyi Tang, Mengda He, Lei Bu, Shengchao Qin, Yifeng Zeng
Abstract— In this paper, we revisit model predictive control (MPC) for the classical wheeled mobile robot (WMR)
navigation problem. We prove that the reachable set basedhierarchical MPC (HMPC), a state-of-the-art MPC, cannot
handle WMR navigation in theory due to the non-existence of
non-trivial linear system with an under-approximate reachable
set of WMR. Nevertheless, we propose a virtual linear leader
guided MPC (VLL-MPC) to enable HMPC structure. Different
from current HMPCs, we use a virtual linear system with anunder-approximate path set rather than the traditional trace
set to guide the WMR. We provide a valid construction of
the virtual linear leader. We prove the stability of VLL-MPC,
and discuss its complexity. In the experiment, we demonstrate
the advantage of VLL-MPC empirically by comparing it with
NMPC, LMPC and anytime RRT* in several scenarios.

I. I NTRODUCTION
The wheeled mobile robot (WMR) navigation problem
considered in this paper is to drive a nonlinear dynamical
WMR governed by a discrete difference equation, from its
initial state to the goal state by on-line real-timely tuning its
control input, in a continuous state and input space. When
there is no available reference path, a successful navigationmust ensure the WMR can converge to the goal state, referred
to as stability. Meanwhile, the real-time nature requires thecomputation as promptly as possible, referred to as efficiency.
An intuitive solution to the problem is to combine themethods of trace planning and trace tracking as a whole.
Unfortunately, the distinctive nature of the problem prevents
many mutual techniques from being applied. Due to thecontinuity of state space, traditional heuristic-search based
techniques designed for the planning problem of discrete
space, such as A* like methods [1], [2] are not applicable.
The off-line planning [3], [4] and tracking techniques [5],[6] cannot satisfy the efficiency requirement enforced by
the online planning. Besides, the approaches for systemsof ordinary differential equations (ODE) [7] are unable tohandle a WMR as the property of universal trace following
no longer holds for discrete systems.
Current approaches solve this problem by extending theoff-line planning and tracking to the online, which mainly
fall into two categories: anytime rapidly exploring random
Chao Huang, Xin Chen, Enyi Tang and Lei Bu are with the
Department of Computer Science and Technology, Nanjing University, Nanjing, Jiangsu, China. chaohuang@seg.nju.edu.cn,{chenxin,eytang,bulei}@nju.edu.cn.
Chao Huang is also with the Department of Electrical Engineering andComputer Science, Northwestern University, Evanston, IL, USA.
Mengda He, Shengchao Qin and Yifeng Zeng are with the School ofComputing, Teesside University, UK. Shengchao Qin is also with Shenzhen
University. {m.he, s.qin, y.zeng}@tees.ac.uk.
Corresponding author: Xin Chentree (anytime RRT) and model predictive control (MPC).
Anytime RRT is the online version of RRT [8], [9] fortrace planning. Along with the trace techniques [5], [6], it
can be used to solve robot navigation problem. However,regardless of the current result that even the off-line RRTcannot ensure the probabilistic stability for nonlinear robots
with common settings [10], there is currently no theoreticalstability guarantee on anytime RRT.
MPC is one of the main stream methodologies in robotics
community for the navigation/control problem of various
systems [11], [12], [13], [14], [15], [16]. Its main idea is
at each instant t, to predict the behavior of a system, overfinite future steps, namely, prediction horizon, by solving
a programming problem based on the dynamic model andcollision avoidance specification, and only apply the firstpredicted action. This procedure will repeat at the next
control instant until a certain convergent criteria is met.
Basic MPC approach for WMR is to directly encode all thenonlinear constraints (NMPC) and jointly generate the trace
and inputs [17]. Due to the intrinsic complexity of nonlinear
programming, NMPC is time consuming.
In recent years, hierarchical MPC (HMPC) as one of
the advanced MPC, receives more attention. HMPCs adopt
a two-level structure. In the upper level, a simple linear
dynamic system is used to help plan the trace by linear MPC,
and then the original system tries to follow the trace in thelower level. Such hierarchical treatment has shown a great
advantage on efficiency for linear/piecewise affine systems[18], [19], [20]. The key of HMPC is to guarantee the linear
system has a “smaller” state reachable set than the original
system, such that any trace planned by the linear system isachievable for the original system. It is known as the underapproximation problem [21], [22].
However, in this paper, we show that the traditional
state reachable-set based HMPCs are inapplicable for WMRnavigation due to the nonlinearity introduced by the statevariable θ, denoting the direction. On one hand, we prove that
there exists one and only one trivial linear system that has anunder-approximate reachable set of the WMR, but can only
steer in place (see Theorem 1). On the other hand, simply
linearizing the WMR dynamic to make classical HMPCs(LMPC) applicable will inevitably introduce approximation
error as a disturbance and is unable to ensure stability either.
In order to leverage the high efficiency of HMPC, anovel Virtual Linear Leader guided Model Predictive Control(VLL-MPC) is proposed for WMR navigation. In the problem, the target state consists of both the target position andthe target direction. Following the philosophy of successive

approaching, it is possible to firstly drive a WMR to a point
very near to the target position, and then guide it to thefinal state by setting the speed and the angular velocity
properly (see Proposition 1). As a result, the navigation
problem can be safely converted into the problem of finding apath convergent to the final position. The advantage comingfrom such conversion is that the direction variable θ canbe safely ignored in the planning stage such that it is easy
to build a linear system (VLL) under-approximating thereachable set of the WMR. As the key step of VLL-MPC,
the construction of the VLL for the dynamic of WMR
is elaborated whose validity is strictly proved. Benefitingfrom the HMPC structure, VLL-MPC’s stability and highefficiency can be theoretically assured.
In the paper, we make the following contributions:• We prove that reachable set based HMPCs cannot
handle WMR navigation in theory;• We propose a novel VLL-MPC to enable the HMPC
framework for the WMR navigation and give a construction of the virtual linear leader, which is the coreof VLL-MPC;• We give a sufficient condition to theoretically ensure
the stability of VLL-MPC and discuss the complexity;• We demonstrate the performance of VLL-MPC in several scenarios and show its advantages empirically by acomparison with NMPC, LMPC and anytime RRT*.
II. P ROBLEM F ORMULATIONA WMR follows the discrete dynamic model [23], [5]:[xi , yi ]:qi (t + 1) = fi (qi (t)),

i = 1, · · · , n.

(4)

Let qe = [q1 , · · · , qn ]. We use the following compact form
to represent the environment dynamic:qe (t + 1) = fe (qe (t));(5)

The distance between the WMR and any obstacle should not
be over a given dsafe to avoid collision:
k[x, y] − [xi , yi ]k∞ ≥ dsafe ,i = 1, · · · , n.

(6)Subsequently we formulate the WMR navigation problem
based on the above model of WMR and environment:
Problem 1 (WMR Navigation). Given a WMR W, where• the dynamic model is Equation (1) and its input constraint described as Constraint (2);• an initial state is q(0) = [x(0), y(0), θ(t)];00 0 0• a goal state is q = [x , y , θ ],an environment E, where• the dynamic model is Equation (5);• an initial state is qe (0);the collision avoidance specification described as Constraint (6) and a convergence error bound , the WMRnavigation problem is to find a control strategy to determine
a path ρW (q0 ) by on-line generating a control sequence{u(t)}∞
t=0 such that W can reach Bq 0 () without collision,
where Bx (r) denotes a ball around x with radius r.
Two key properties stability [26] and efficiency are defined
as follows.

x(t + 1) = x(t) + v(t) cos θ(t)∆Ty(t + 1) = y(t) + v(t) sin θ(t)∆Tθ(t + 1) = θ(t) + ω(t)∆TDefinition 1 (Stability). A control scheme is stable for a
WMR navigation problem 1, iff starting from the initial stateq(0), the path ρW (q0 ) of computed by the control scheme
reaches Bq0 ().

where x and y denote the position under Cartesian coordinate, θ denote the direction, v and ω are the forward
and steering velocity respectively. This model can be also
represented in a compact form:

Definition 2 (Efficiency). The efficiency of an on-line control
scheme is defined as its average computation time at eachsampling instant.

q(t + 1) = f (q(t), u(t)),

III. R EVISIT MPC S FOR WMR NAVIGATION(1)

where q = [x, y, θ] is the state variable, u = [v, ω] is the
input variable, t denotes the t-th sampling instant and ∆T isthe sampling period. Due to the limit of the engine power,
the velocity is bounded by the input constraint:u ∈ U , |v| ≤ Vbound .

(2)

The WMR’s behavior is either defined by a trace or a path[24], [25]. A trace of W, denoted as trW (q), is a finite statetf
sequence {q(t)}t=0
satisfying Constraint (2), derived fromf , while q(0) = q and tf is an arbitrary natural number. A
path ρW (q) only considers the position part of a trace, i.e.
tf{[x(t), y(t)]}t=0. TrW (q) and PW (q) denote the set of all
traces and paths from q, respectively.
Such a robot needs to navigate in a dynamical environment, including both moving and stationary obstacles. We
consider the linear convex space, that is, the overall state
space of the environment can be described as a polytope Q:
q ∈ Q , P × [−π, π].

(3)where P represents the position space for [x, y].
We assume that there are n obstacles with linear dynamics,
and the state qi of the i-th obstacles include its position

Let us first revisit MPCs, including NMPC and the stateof-the-art HMPC for WMR navigation from an unified
perspective of reachable set [27], [28] to motivate our idea.
Definition 3 (Reachable set). The reachable set of W fromq after the next sampling period is defined as ReachW (q) ={q 0 |∃u ∈ U, q 0 = f (q, u)}. Specially, a set ReachW (q) ⊆ReachW (q) is called an under-approximate reachable set.
In NMPC, at each sampling instant t, an optimization
problem needs to be solved online to predict the behaviorof the robot, which involves the robot dynamic and all
the relevant constraints. Figure 1 shows an example of
how NMPC works at each sampling instant t. In NMPC,
the original nonlinear dynamic is directly encoded in the
optimization problem, which indicates the exact reachable set(irregular black shape) is implicitly considered. The yielded
nonlinear programming makes NMPC time consuming.
Remark 1. Notably in practice, rather than computing thereachable set, NMPC only finds a (optimal) feasible point,𝑞 𝑡𝑣 𝑡 ∆𝑇
𝜃 𝑡
𝑞 𝑡

𝑡

1|𝑡

2|𝑡

which has lower computation complexity. Here, we leveragereachable set to provide a mathematical intuition of NMPC.
In Section V, we will elaborate the computation complexity.
A naive idea is to explore if we can find a linear systemwith an under-approximate reachable set of the WMR, suchthat the WMR can reach every state planned within theunder-approximate reachable set. Then such linear system
can be used to plan the trace in the upper level by MPC,
while the actual input of the WMR is then computed todrive the WMR to follow the trace. This idea is also
known as hierarchical MPC (HMPC) and adopted in recent
works for linear/piecewise affine systems [18], [19], [20].
Intuitively, the system that only does pivot steering meetssuch requirement:x(t + 1) = x(t), y(t + 1) = y(t), θ(t + 1) = A3 θ(t) + u3 .

However, it cannot be used for trace planning and thus wecall it trivial. Unfortunately, we find that there exists no nontrivial linear under-approximation for WMR:
Theorem 1. For a WMR with dynamic model (1) and input
constraint (2), there exists no linear system with an underapproximate reachable set, except for the trivial one.
Proof. Assume there exists such a system and WLOG, its
dynamic is:x(t + 1) = A1 x(t) + u1 , y(t + 1) = A2 y(t) + u2 ,θ(t + 1) = A3 θ(t) + u3 .

where Ai and ui are the coefficient and input for eachdimension respectively. Consider the special case q(t) =[0, 0, 0]. Recall the dynamic of WMR, we know
ReachW (q(t)) = [−Vbound ∆T, −Vbound ∆T ] × {0} × [−π, π].

To meet the under-approximation requirement, we have
0 = y(t + 1) = A2 y(t) + u2 = u2

Similarly, when q(t) = [0, 0, π/2], we can obtain that u1 =0. Thus this system is trivial.
We may simply linearize the nonlinear WMR dynamic
and use the linear dynamic for planning to enable HMPC
structure, ignoring the requirement of under-approximation.
It will lead to the linear approximate reachable set with thelinearization error (red polytope). To differ it from the typical
HMPC, we name it LMPC. However, such linearization isprecise only locally around the given point. Since one cannot“predict” the linearization of the system at future steps, thelinearization error will inevitably accumulate over time and

𝜃 𝑡

1

1
𝜃 𝑡

1

3|𝑡

Fig. 1: Methodologies of NMPC and LMPC at sampling instant t:
The blue rectangle is the state space. Black and red shape represent
the exact and linear-approximate reachable set. The purple points
denote the predicted trace in the next three steps. The symbol
i|t, i = 1, 2, 3 denotes the i-th step predicted at t.

1

𝜃 𝑡

1

𝜔 𝑡

1 ∆𝑇

𝑞 𝑡

Fig. 2: Relation between the position and input variables: The
position of the WMR at t + 1 is determined by the direction θ(t)and the linear velocity v(t), while θ(t) is determined by ω(t − 1).
Thus, the position at t + 1 is determined by v(t) and ω(t − 1).

makes the robot system unpredictable (See Figure 1).
IV. VLL-MPC
Our idea is based on an interesting observation that once
WMR’s position is very closed to the goal position, we canalways let the overall state also close to the goal state:
Proposition 1. If k[x(t), y(t)] − [x0 , y 0 ]k ≤ , then let v(t) =0, ω(t) = (θ0 − θ(t))/∆T , we have kq(t + 1) − q 0 k ≤ .
Thus the original problem can be converted into determining a convergent path.
Based on Proposition 1, we propose a virtual linear
leader based model predictive control (VLL-MPC) scheme
to enable HMPC structure. We construct a virtual robot R
with state variable qR , which has linear dynamic and thesmaller path set from any state q than the WMR W , that is
PR (q) ⊆ PW (q). We refer to R as a virtual linear leader.
Then following the HMPC manner, we online plan the pathbased on the dynamic of R by linear MPC. With a larger
path set, the WMR is able to strictly follow the planned
path. Thus, we can make W converge to the goal state q 0 by0
= [x0 , y 0 ]. In the following, we firstletting R converge to qR
introduce the construction of the virtual linear leader. Thenthe detailed VLL-MPC will be given.
A. Construct a Virtual Linear Leader
Observing the dynamic of W as Equation (1), we can findthat the position at instant t + 1 is determined by the forward
velocity v(t) and steering velocity ω(t−1) (See Figure 2). It
motivates us to construct a virtual linear leader R as follows:x(t + 1) = x(t) + v1 (t)∆T,y(t + 1) = y(t) + v2 (t)∆T,or in a compact form:qR (t + 1) = fR (qR (t), uR (t)),(7)
where qR = [x, y] is the state, uR = [v1 , v2 ] is the input. The

input constraint is:|v1 |, |v2 | ≤√2Vbound ,2

v1 (0) = v2 (0) = 0.

(8)

Now we point out the validity of our construction R.
Theorem 2. For a WMR W with the dynamic model (1)and the input constraint (2), and the virtual linear leader
R with the dynamic model (7) and the input constraint (8),∀q(0) = [x(0), y(0), θ(0)] ∈ Q, let qR (0) = [x(0), y(0)],then we have PR (qR (0)) ⊆ PW (q(0)).

Compute the input: The controller then derives the input of
the WMR W based on the state qR (t + 1) and qR (t + 2) of
R. That is, compute u(t) = [v(t), ω(t)] according to the stateqR (t+1) and qR (t+2) by (9) and apply it on W by (1) suchthat W reaches q(t+1), where [x(t+1), y(t+1)] = qR (t+1).
This procedure will be repeated at the next sampling
instant t+1 based on until the q(t) meets the convergencecriteria .

Proof. Given any path
ρR (qR (0)) = {[x(0), y(0)], · · · , [x(t0 ), y(t0 )]} ∈ PR (q(0)),

where t0 is the terminal instant. Let∆x(t) , x(t + 1) − x(t), ∆y(t) , y(t + 1) − y(t),p∆D(t) , (∆x(t))2 + (∆y(t))2 ,∆y(t + 1)
γ(t + 1) = arcsin, if ∆D(t + 1) 6= 0.
∆D(t + 1)

We construct the input sequence of W as
µ = [v(0), ω(0)], · · · , [v(t0 − 1), ω(t0 − 1)],

wherev(t) = ∆D(t)/∆T, t = 0, · · · , t0 − 1∆D(t+1)=0, t<t0 −10,(γ(t+1)−θ(t))/∆T,∆x(t+1)≥0, t<t0 −1ω(t)=(π−γ(t+1)−θ(t))/∆T, ∆x(t+1)<0, t<t0 −1(θ0 −θ(t0 −1))/∆T,
t=t0 −1(9)Remark 3. Note that an obstacle here can be either uncontrollable (e.g. a building) or controllable (e.g. a robot withlinear dynamics). Specially, when handling a controllable
robot obstacle, our approach can be naturally integrated withexisting distributed MPC schemes [18], [33].
V. A LGORITHM A NALYSIS

It is easy to check that v(t) is valid in terms of Constraint(2) and the path ρW (q(0)) of W generated by µ equals
ρR (qR (0)).

[Stability] Theorem 2 ensures that the path planned basedon R can be achieved by W . Thus the stability of VLL-MPC
is determined by the linear MPC law (10), (11).

Remark 2. Note that ω(t0 − 1) does not affect the establishment of Theorem 2, thus can be arbitrary. However,when qR (t0 ) approaches the goal position, according to
Proposition 1, our assignment of ω(t0 − 1) can let θ(t0 ) be
the goal direction.

Lemma 1. VLL-MPC scheme stabilizes the WMR W iff the
MPC law (10), (11) stabilizes the virtual linear leader R.

B. Control Scheme
As indicated by Figure 2 and Equation (9), W ’s input at t
is computed based on the states of R at both t + 1 and t + 2,
thus we need to compute the planned path two steps ahead,
which is significantly different from the current HMPC.
In detail, at each sampling instant t, the WMR W knows
its current state q(t), while the virtual linear robot R knows
its state qR (t + 1). the control scheme consists of two
successive steps:
Compute the planned state at t + 2: The controller tries toplan a goal state qR (t + 2) for R at t + 2 by linear MPC:minq̃R (H|t+1)

H−1
X

0
+lH (qR (H|t+1))
qR (k|t+1)−qR
∞

k=0


qR (k|t+1) = fR (q√R (k−1|t+1), uR (k−1|t+1)), 1≤k≤H,


√


2

|u
(k−1|t+1)|≤[
Vbound ∆T, 22 Vbound ∆T ], 1≤k≤H,
R

2


qR (k|t+1)∈Q, 1≤k≤H,s.t.

qe (t+k+1)=fe (qe (t+k)), 1≤k≤H,




kq

R (k|t+1)−qi (k|t+1)k∞ ≥dsafe , 1≤i≤n, 1≤k≤H,qR (H|t+1)∈Pf , qR (0|t+1) = qR (t+1).
(10)

where H is the prediction horizon, the notation (·)(k|t + 1)
denotes the predictive value at the (t+1+k)-th collaboration
instant computed at time t + 1, q̃(H|t + 1) , qR (1|t +1), · · · , qR (H|t + 1) denotes the decision variables, andlH (qR (H|t + 1)) , c kqR (H|t + 1) − q 0 k∞ is the terminal
cost and c is an user-defined coefficient. Pf is the terminal
constraint set. lH and Pf are both used to ensure stability.
∗∗
Let qR(1|t + 1), ..., qR(H|t + 1) denote the optimal solution.
∗
The first sample qR (1|t + 1) will be used as the desired stateqR (t + 2) for R at the instant t+2:∗
(1|t+1).
qR (t + 2) = qR

The VLL R then moves to qR (t + 2).

(11)

In the theory of linear MPC, the terminal constraint set andcost function methods are widely used to ensure the stability
by properly choosing its stable parameters, a terminal costlH , a terminal constraint set Pf and a controller function
κ(·) [26], [29], [30]. We adopt the strategy of parameter
configuration in [29] and give the following theorem:
0}, then
Theorem 3. ∀H ∈ N+ , c ∈ R+ , such that Pf = {qR
the VLL-MPC scheme stabilizes the WMR W .

Proof. The MPC law (10), (11) stabilizes the virtual linear
leader R with this parameter configuration [29]. According
to Lemma 1, it is equivalent to that VLL-MPC scheme
stabilizes the WMR W .
[Complexity] Observe the control scheme of VLL-MPC.
The overall computation consists of solving the optimization
problem (10) and computing the input (9). It is obvious that
Equation (9) is an elementary function with fixed numberof operations, which means that computing Equation (9)takes constant time. Thus, the time complexity of overall
VLL-MPC scheme is determined by the optimization (10).
Based on the current works, we show the advantage of VLLMPC on efficiency compared with NMPC in theory by thefollowing conclusion on complexity.
Theorem 4. NMPC is NP-hard. VLL-MPC is NP-complete
in general. Specially, VLL-MPC is of polynomial time if noobstacle exists.
Proof. NMPC needs to solve a nonlinear programming(NLP). It has been shown in [31] that solving a NLP withquadratic objective and non-convex constraints is NP-hard.
In general, the optimization problem (10) is a mixed integer
programming (MILP), which is NP-complete. If there is no
obstacle, (10) degenerates to a linear programming, which isof polynomial time.

50

50

50

50

45

45

45

45

40

40

40

40

35

35

35

35

30

30

30

30

25

25

25

25

20

20

20

20

15

15

15

15

10

10

10

10

5

5

5

0

0
0

10

20

30

40

50

5

0
0

10

(a) Free Space:VLL-MPC

20

30

40

50

0
0

10

(b) Free Space: NMPC

20

30

40

50

0

(c) Free Space: LMPC

50

50

50

50

45

45

45

45

40

40

40

40

35

35

35

35

30

30

30

30

25

25

25

25

20

20

20

20

15

15

15

15

10

10

10

10

5

5

5

0

0
0

10

20

30

40

50

10

(e) Stationary Obs: VLL-MPC
50

20

30

40

50

40

10

20

30

40

(g) Stationary Obs: LMPC

40

35

35

30

30

30

30

25

25

1

25

20

20

20

20

15

15

15

15

10

10

10

10

5

5

5

2
0

2

0
0

10

20

30

40

50

(i) Moving Obs: VLL-MPC

0

20

30

40

50

(j) Moving Obs: NMPC

30

40

50

5

0
10

20

40

35

1

10

45

2

35

25

50

(h) StationaryObs: Anytime RRT*

1

40

2

40

50

45

1

2

0

50

50

45

1

30

0

0

(f) Stationary Obs: NMPC
50

45

20

5

0
0

10

(d) Free Space: Anytime RRT*

0
0

10

20

30

40

(k) Moving Obs: LMPC

50

0

10

20

30

40

50

(l) Moving Obs: Anytime RRT*

Fig. 3: Robot traces in different scenarios[Sub-optimality] Note that in Equation (10), we restrict
the behavior of W to a linear dynamic system with underapproximate path set. Thus, the obtained control scheme
is only optimal with respect to the linear model, but suboptimal for the original nonlinear model. We believe it is afair cost of the efficiency improvement and maintaining thestability.
VI. S IMULATION
We compare our VLL-MPC method with state-of-theart navigation control algorithms, including NMPC [32],LMPC [23] and anytime RRT* [8]. As mentioned previously,
NMPC has theoretical guarantee on the stability under certain
pre-conditions [26], LMPC cannot ensure stability due to theintroduction of additional disturbance (approximation error),while anytime RRT* no longer maintains the probabilistic
stability. We empirically demonstrate it by simulation.
A. Experimental Setup
We consider a practical scenario, autonomous parking
scenario, where the parameters of WMR are set as Vbound =2, ∆T = 1. We carefully choose three typical scenarios,
which are named Free Space scenario, Stationary Obstacle
scenario, and Moving Obstacle scenario, to demonstrate thepros and cons of each approach. Details on scenario setting
are described as follows:[Free Space Scenario]: Free Space scenario is the simplest
case, where there is no constraint on robot position and no
obstacle in the environment. The WMR can move freely on
the plane. The initial state of the robot is [3, 47, 0] and thegoal state is [36, 25, 1.5π]. We set the prediction horizon
H=30, c=1. With respect to anytime RRT*, we allow it toexecute no longer than 10 seconds at each sampling instant.
[Stationary Obstacle Scenario]: Stationary Obstacle scenario is more close to a real parking lot than Free Space
scenario, with constraints on robot positions and stationary
obstacles in the environment. The whole parking lot is a[0, 56] × [0, 50] rectangle area. There are 15 parking spacesin the parking lot, which arrange into three rows. Some ofthe parking spaces have already been occupied by WMRs.
The concerned WMR needs to reach the goal parking spacefrom the entrance without entering any other parking spaceor going out of the parking lot area. The initial state and thegoal state are the same as in Free Space Scenario. We set
the prediction horizon H=30, c=1. Considering the all thepossible situations, we conservatively set dsafe = 6.25. The
maximal execution time for anytime RRT* is 20 seconds.
[Moving Obstacle Scenario]: Moving Obstacle scenario isfurther close to a real parking lot than Stationary Obsta-

cle one. There are still position constraints and stationary
obstacles. However, except for the WMR 1 we concern,
there is another WMR, WMR 2, in the environment. WMR
1 needs to reach the goal parking space as in Stationary
Obstacle scenario, while WMR 2 wants to achieve thatstate [3, 3, π], denoting the exit, from the current parking
space [28, 39, 1.5π]. One WMR should avoid to collide withanother during the process. In this case, a robot needs toact by guessing another robot’s behavior. As mentioned in
Remark 3, we therefore integrate each MPC with a standard
distributed control method proposed in [33] to tackle it. We
set H=60, c=1, dsafe = 6.25. The maximal execution time
for anytime RRT* in this scenario is 60 seconds.
All the simulations were performed on a computerequipped with 8 GB RAM and an Intel Core i5 4570 CPU
under Windows. The LP and MILP problems were solvedby the Gurobi solver [34]. NLP problems encountered in the
NMPC were solved by Tomlab toolbox [35].
B. Result Analysis
The traces computed by various algorithms are shown in
Fig 3. In all the sub-figures, a WMR is drawn as a rectangle,
where the short purple edge denotes the back and the redpoint denotes the center. The blue and green *’s denotethe initial position and the goal positions respectively. The
white space denotes the permitted moving area for WMRs.
In the scenarios with obstacles (Fig 3e-3l), the parking lot isdrawn as a large rectangle with red edges, the yellow areas
denote the parking spaces and the blue rectangles denotethe stationary WMRs. Observing Fig 3a, 3e, 3i, VLL-MPC
completed the navigation task in all the scenarios.
NMPC also finished the simulation (see Fig 3b, 3f, 3j). It is
worthy noting robots moved slowly at some points in Moving
Obstacle scenario. The reason is that the performance of nonlinear solvers relies heavily on the initial guess of decisionvariables while solving NLP problems. In complicated cases,
it is difficult to find an appropriate initial value. When thedefault initial value cannot lead to a solution satisfying thesufficient condition of stability, we will randomly pick an
initial value in the neighborhood of the current state.
LMPC did not finish any scenario (See Fig 3c, 3g, 3k).
As mentioned in Section III, linearization is precise only
locally around the given equilibrium point. Since one cannot“predict” the linearization of the system at future steps, thelinearization error will will inevitably accumulate over timeand make the robot dynamic uncontrollable. As a result,
the robot was going to leave the default observable area in
Figure 3c, while in Figure 3g and 3k, the WMR just moved
chaotically around the initial position.
Notably there exists randomness in RRT-based methods,
therefore we ran Anytime RRT* 10 times for each scenario,
where Fig 3d, 3h, 3l show the best results. Anytime RRT*
succeeded in the Free Space scenario but failed in othertwo. More specifically, the robot stuck for a long time froma certain instant in the Stationary and Moving Obstacle
scenarios. In fact, there is currently no explicit theoreticalresult on the stability of anytime version of RRT or RRT*.

TABLE I: Comparison on Effiency (Average Computation Time at Each Instant in Seconds)
VLL-MPC
NMPC
LMPC
Anytime RRT*1

Free
0.165
1.463
0.129
10.00

StationaryObs
0.768
36.64
0.671
20.00

MovingObs
2.310
131.5
2.563
60.00

1 Considering the characteristics of sampling based algo-rithms, we give a computation time bound for the anytime
RRT* in each scenario. Such a setting allows the anytime
RRT* to sample at least 800, 1500, 4200 points each timeon our platform in Free space, Stationary Obstacle andMoving Obstacle scenarios respectively.

In [8], only the situations with the pre-specified reference
trace were discussed in the experiments. A possible reason
is that at each instant, the number of sampling is small due tothe limited computation time and results at previous instants
are dropped, which makes the sampling set not dense in thestate space and probabilistic stability no longer hold. Even
though RRT based approaches show the great advantage in
off-line motion planning, it still needs more effort to ensure
the stability for on-line planning.
Table I shows the efficiency, i.e. the average computation
time at each instant, of all the algorithms in terms of different
scenarios. In the following, we explain the table in detail.
• Compare with NMPC: VLL-MPC achieved at least
88.7% improvement, and the computation time of
NMPC grew rapidly with the complexity of scenarios.
The result fitted well our theoretical conclusion in
Section V.
• Compare with LMPC: Due to that the optimization
problems involved in our VLL-MPC and LMPC areboth linear programming and of the same scale, it is
not surprised that they share the similar efficiency.
• Compare with Anytime RRT*: Anytime RRT* has ahigh chance to fail the experiment, even though it is
1 order-of-magnitude longer than the time required by
VLL-MPC. It is worthy noting that we may let it stableby specifying a sufficient large time bound. However,such a setting makes anytime RRT* degenerate to the
traditional off-line version.
VII. C ONCLUSION
In this paper, we show reachable set based HMPC, one of
the advanced MPC techniques for robot control, cannot handle WMR navigation. Then we propose VLL-MPC to enable
HMPC structure to improve the efficiency compared withNMPC while maintaining stability. Future work includes theintegrating VLL-MPC with RRT for off-line path planning.
ACKNOWLEDGEMENT
This research was supported by the National Key Researchand Development Program of China (No.2017YFA0700604),
and the National Natural Science Foundation of China(No.61690204, No.61772347, No.61836005, No.61572249,
No.61772260). Dr. Yifeng Zeng is also supported by the
EPSRC Grant (EP/S011609/1).

R EFERENCES[1] G. Wagner and H. Choset, “M*: A complete multirobot path planning algorithm with performance bounds,” in IROS. IEEE, 2011,
Conference Proceedings, pp. 3260–3267.
[2] W. Zeng and R. L. Church, “Finding shortest paths on real road
networks: the case for a*,” International journal of geographicalinformation science, vol. 23, no. 4, pp. 531–543, 2009.
[3] S. M. LaValle, “Rapidly-exploring random trees: A new tool for path
planning,” 1998.
[4] S. Karaman and E. Frazzoli, “Sampling-based algorithms for optimal
motion planning,” Int. J. Rob. Res., vol. 30, no. 7, pp. 846–894, 2011.
[5] Z. Li, J. Deng, R. Lu, Y. Xu, J. Bai, and C. Y. Su, “Trajectorytracking control of mobile robot systems incorporating neural-dynamic
optimized model predictive approach,” IEEE Transactions on SMC:
Systems, vol. 46, no. 6, pp. 740–749, 2016.
[6] C. J. Ostafew, A. P. Schoellig, T. D. Barfoot, and J. Collier,“Learning-based nonlinear model predictive control to improvevision-based mobile robot path tracking,” Journal of Field Robotics,
vol. 33, no. 1, pp. 133–152, 2016. [Online]. Available: http://dx.doi.org/10.1002/rob.21587[7] S. M. LaValle, Planning algorithms. Cambridge university press,
2006.
[8] S. Karaman, M. R. Walter, A. Perez, E. Frazzoli, and S. Teller,“Anytime motion planning using the rrt,” in ICRA. IEEE, 2011,
Conference Proceedings, pp. 1478–1483.
[9] J. D. Hernández, E. Vidal, G. Vallicrosa, E. Galceran, and M. Carreras,“Online path planning for autonomous underwater vehicles in unknown environments,” in ICRA. IEEE, 2015, Conference Proceedings,
pp. 1152–1157.
[10] T. Kunz and M. Stilman, “Kinodynamic rrts with fixed time step andbest-input extension are not probabilistically complete,” in Algorithmic
foundations of robotics XI. Springer, 2015, pp. 233–244.
[11] M. Ramirez, M. Papasimeon, L. Benke, N. Lipovetzky, T. Miller, and
A. R. Pearce, “Real–time uav maneuvering via automated planning insimulations,” in IJCAI. AAAI Press, 2017, Conference Proceedings,
pp. 5243–5245.
[12] E. Fernández-González, E. Karpas, and B. C. Williams, “Mixed
discrete-continuous heuristic generative planning based on flow tubes,”
in IJCAI, 2015, Conference Proceedings, pp. 1565–1572.
[13] G. Williams, N. Wagener, B. Goldfain, P. Drews, J. M. Rehg, B. Boots,
and E. A. Theodorou, “Information theoretic mpc for model-based
reinforcement learning,” in 2017 IEEE International Conference on
Robotics and Automation (ICRA). IEEE, 2017, pp. 1714–1721.
[14] F. R. Hogan, E. R. Grau, and A. Rodriguez, “Reactive planar manipulation with convex hybrid mpc,” in 2018 IEEE International Conferenceon Robotics and Automation (ICRA). IEEE, 2018, pp. 247–253.
[15] A. Tanguy, D. De Simone, A. I. Comport, G. Oriolo, and A. Kheddar,“Closed-loop mpc with dense visual slam-stability through reactive
stepping,” in 2019 International Conference on Robotics and Automation (ICRA). IEEE, 2019, pp. 1397–1403.
[16] I. B. Hagen, D. K. M. Kufoalor, E. F. Brekke, and T. A. Johansen,“Mpc-based collision avoidance strategy for existing marine vessel guidance systems,” in 2018 IEEE International Conference on
Robotics and Automation (ICRA). IEEE, 2018, pp. 7618–7623.
[22] E. Goubault, O. Mullier, S. Putot, and M. Kieffer, “Inner approximatedreachability analysis,” in hscc. ACM, 2014, Conference Proceedings,
pp. 163–172.

[17] H. A. v. Essen and H. Nijmeijer, “Non-linear model predictive control
for constrained mobile robots,” in ECC, 2001, Conference Proceedings, pp. 1157–1162.
[18] C. Huang, X. Chen, Y. Zhang, S. Qin, Y. Zeng, and X. Li, “Hierarchical model predictive control for multi-robot navigation,” in IJCAI.
AAAI Press, 2016, Conference Proceedings, pp. 3140–3146.
[19] B. Picasso, X. Zhang, and R. Scattolini, “Hierarchical model
predictive control of independent systems with joint constraints,”
Automatica, vol. 74, pp. 99–106, 2016. [Online]. Available:
http://www.sciencedirect.com/science/article/pii/S0005109816302989[20] C. Huang, X. Chen, Y. Zhang, S. Qin, Y. Zeng, and X. Li, “Switchedlinear multi-robot navigation using hierarchical model predictive
control,” in IJCAI, 2017, Conference Proceedings, pp. 4331–4337.
[Online]. Available: 10.24963/ijcai.2017/605[21] E. Asarin, O. Bournez, T. Dang, and O. Maler, “Approximate reachability analysis of piecewise-linear dynamical systems,” in HSCC.
Springer, 2000, Conference Proceedings, pp. 20–31.
[23] F. Künhe, J. Gomes, and W. Fetter, “Mobile robot trajectory tracking
using model predictive control,” in LARS, 2005, Conference Proceedings.
[24] S. Mitra, “A verification framework for hybrid systems,” Thesis, 2007.
[25] R. Alur, R. Grosu, I. Lee, and O. Sokolsky, “Compositional refinement for hierarchical hybrid systems,” in HSCC. Springer, 2001,
Conference Proceedings, pp. 33–48.
[26] D. Q. Mayne, J. B. Rawlings, C. V. Rao, and P. O. M. Scokaert,“Constrained model predictive control: Stability and optimality,” Automatica, vol. 36, no. 6, pp. 789–814, 2000.
[27] C. Huang, X. Chen, W. Lin, Z. Yang, and X. Li, “Probabilistic safety
verification of stochastic hybrid systems using barrier certificates,”
ACM Transactions on Embedded Computing Systems (TECS), vol. 16,
no. 5s, pp. 1–19, 2017.
[28] Z. Yang, C. Huang, X. Chen, W. Lin, and Z. Liu, “A linear programming relaxation based approach for generating barrier certificatesof hybrid systems,” in International Symposium on Formal Methods.
Springer, 2016, pp. 721–738.
[29] A. Bemporad and M. Morari, “Control of systems integrating logic,
dynamics, and constraints,” Automatica, vol. 35, no. 3, pp. 407–427,
1999.
[30] Y. Kuwata and J. P. How, “Stable trajectory design for highly constrained environments using receding horizon control,” in ACC, vol. 1.
IEEE, 2004, Conference Proceedings, pp. 902–907.
[31] R. Hemmecke, M. Köppe, J. Lee, and R. Weismantel, “Nonlinear
integer programming,” in 50 Years of Integer Programming 1958-2008.
Springer, 2010, pp. 561–618.
[32] A. Boccia, L. Grüne, and K. Worthmann, “Stability andfeasibility of state constrained mpc without stabilizing terminal
constraints,” Systems & Control Letters, vol. 72, pp. 14–21, 2014.
[Online]. Available: http://www.sciencedirect.com/science/article/pii/
S0167691114001595[33] T. Keviczky, F. Borrelli, and G. J. Balas, “A study on decentralized
receding horizon control for decoupled systems,” in ACC, vol. 6.
IEEE, 2004, Conference Proceedings, pp. 4921–4926.
[34] I. Gurobi Optimization, “Gurobi optimizer reference manual,” 2016.
[Online]. Available: http://www.gurobi.com[35] K. Holmström and M. M. Edvall, The tomlab optimization environment. Springer, 2004, pp. 369–376.