motion planning based on uncertain robot states in dynamic environments : a receding horizon control...
DESCRIPTION
This thesis is concerned with trajectory generation for robots in dynamic environments with relatively narrow passages. In particular, this thesis aims at developing motion planning schemes using receding horizon control (RHC) and mixed-integer linear programming (MILP). The thesis is constructed of two phases. In the first phase, a general nonlinear RHC framework is developed utilizing existing algorithms for motion planning of a robot with uncertain states. In this phase, the motion planning problem in the presence of arbitrary shaped obstacles is tackled using nonlinear system and measurement equations. This method is then adopted to solve the problem of cooperating aerial and ground vehicles. An important application of the latter is automated wildfire suppression that is investigated as a case study in this thesis. Simulation results demonstrate that the nonlinear RHC algorithm can effectively compute an optimal trajectory while handling the system uncertainties and constraints. In the second phase, motion planning schemes for robots with uncertain states are developed under more specific assumptions (i.e., linear system and measurement equations and convex polyhedral obstacles) using receding horizon MILP (RHMILP). This phase builds upon the existing RHMILP motion planning methods by introducing two additional features including a state estimation algorithm and an obstacle avoidance constraint. First, the state estimation includes a partially closed-loop strategy to estimate the future states of the robot based on the anticipated future state measurements that can in turn help to reduce the uncertainty of the future states. Second, the obstacle avoidance constraint is designed to secure a line connecting each pair of sequential robot positions inside the configuration space (outside the obstacles) at all discrete times, analogous to maintaining line-of-sight (LOS) between each pair of sequential positions of the robot. The LOS-based obstacle avoidance is advantageous in that it can typically provide the same level of safety at time steps larger than those of the conventional approaches; hence creating trajectories which are more efficient at both planning and execution stages. Numerical simulations and experiments indicate that proper inclusion of the future anticipated measurements contributes to a less conservative (more feasible) path.TRANSCRIPT
-
Motion Planning Based on Uncertain Robot States in Dynamic Environments:
A Receding Horizon Control Approach
by
Ali Mohandes
B.Sc., University of Tehran, 2012
A THESIS SUBMITTED IN PARTIAL FULFILLMENT OF
THE REQUIREMENTS FOR THE DEGREE OF
MASTER OF APPLIED SCIENCE
in
THE COLLEGE OF GRADUATE STUDIES
(Mechanical Engineering)
THE UNIVERSITY OF BRITISH COLUMBIA
(Okanagan)
October 2014
Ali Mohandes, 2014
-
ii
Abstract
This thesis is concerned with trajectory generation for robots in dynamic environments with
relatively narrow passages. In particular, this thesis aims at developing motion planning schemes
using receding horizon control (RHC) and mixed-integer linear programming (MILP). The thesis
is constructed of two phases. In the first phase, a general nonlinear RHC framework is developed
utilizing existing algorithms for motion planning of a robot with uncertain states. In this phase,
the motion planning problem in the presence of arbitrary shaped obstacles is tackled using
nonlinear system and measurement equations. This method is then adopted to solve the problem
of cooperating aerial and ground vehicles. An important application of the latter is automated
wildfire suppression that is investigated as a case study in this thesis. Simulation results
demonstrate that the nonlinear RHC algorithm can effectively compute an optimal trajectory
while handling the system uncertainties and constraints.
In the second phase, motion planning schemes for robots with uncertain states are developed
under more specific assumptions (i.e., linear system and measurement equations and convex
polyhedral obstacles) using receding horizon MILP (RHMILP). This phase builds upon the
existing RHMILP motion planning methods by introducing two additional features including a
state estimation algorithm and an obstacle avoidance constraint. First, the state estimation
includes a partially closed-loop strategy to estimate the future states of the robot based on the
anticipated future state measurements that can in turn help to reduce the uncertainty of the future
states. Second, the obstacle avoidance constraint is designed to secure a line connecting each pair
of sequential robot positions inside the configuration space (outside the obstacles) at all discrete
times, analogous to maintaining line-of-sight (LOS) between each pair of sequential positions of
the robot. The LOS-based obstacle avoidance is advantageous in that it can typically provide the
-
iii
same level of safety at time steps larger than those of the conventional approaches; hence
creating trajectories which are more efficient at both planning and execution stages. Numerical
simulations and experiments indicate that proper inclusion of the future anticipated
measurements contributes to a less conservative (more feasible) path.
-
iv
Preface
This research was conducted in the advanced control and intelligent systems (ACIS) laboratory
at the School of Engineering at the University of British Columbia, Okanagan campus.
A version of Chapter 2 has been accepted (and is going to be presented) at the IEEE-VTC
2014 Conference [1] (A. Mohandes, M. Farrokhsiar, and H. Najjaran, A Motion Planning
SchemeforAutomatedWildfireSuppression,).
A version of Chapter 2 and Chapter 4 has been presented at the CSME International
Congress 2014 [2] (A. Mohandes, M. Farrokhsiar, and H. Najjaran, Robust MILP motion
planning with LOS-basedobstacleavoidanceconstraint,). This paper won the best student paper
award.
A version of Chapter 2 and Chapter 4 has been submitted to the Journal of Autonomous
Robots [3] (A.Mohandes,M.Farrokhsiar,andH.Najjaran,ProbabilisticallySafeMILPMotion
Planning with LOS-basedObstacleAvoidance,).
All of the papers have three authors as follows. I, Ali Mohandes, was the lead investigator,
responsible for conducting research, developing algorithms, programming, and writing the paper.
Morteza Farrokhsiar, the second author and a PhD candidate at the ACIS laboratory, was
involved in the early stages of concept formation and contributed to manuscript edition as a
mentor. Homayoun Najjaran, the third and supervisory author, was involved throughout the
project in concept formation and manuscript edition.
The materials of the prepared conference and journal papers are included in this thesis. In the
case of using figures and tables from the papers, they have been cited. In addition, texts from the
papers have been used throughout this thesis.
-
v
Table of Contents
Abstract ........................................................................................................................................... ii
Preface............................................................................................................................................ iv
Table of Contents ............................................................................................................................ v
List of Tables ................................................................................................................................. ix
List of Figures ................................................................................................................................. x
List of Symbols ............................................................................................................................ xiv
Acknowledgment ........................................................................................................................ xvii
Dedication .................................................................................................................................. xviii
Chapter 1 Introduction .................................................................................................................... 1
1.1 Motivation ......................................................................................................................... 1
1.2 Literature Review .............................................................................................................. 3
1.2.1 Non-RHC Motion Planning Methods .......................................................................... 4
1.2.2 RHC-based Motion Planning ...................................................................................... 6
1.3 Statement of Contributions ................................................................................................ 9
1.4 Thesis Overview .............................................................................................................. 10
Chapter 2 Receding Horizon Trajectory Planning ........................................................................ 12
2.1 Background and Overview .............................................................................................. 12
-
vi
2.2 Problem Statement .......................................................................................................... 14
2.3 State Estimation ............................................................................................................... 14
2.3.1 Filtering (Recursive Bayesian Estimation) ................................................................ 15
2.3.1.1 Linear Gaussian System: Kalman Filter .............................................................. 16
2.3.1.2 Nonlinear Gaussian Systems: Extended Kalman Filter ...................................... 16
2.3.2 Prediction ................................................................................................................... 17
2.3.2.1 Open Loop Prediction ......................................................................................... 18
2.3.2.2 Approximately Closed-loop Prediction ............................................................... 19
2.3.2.3 Partially Closed-loop Prediction ......................................................................... 19
2.4 RHC Algorithm ............................................................................................................... 20
2.4.1 Planning ..................................................................................................................... 21
2.4.1.1 Constraints ........................................................................................................... 22
2.4.2 Execution ................................................................................................................... 24
2.5 Motion Planning for Automated Firefighting: A Case Study ......................................... 24
2.5.1 System Modeling ....................................................................................................... 25
2.5.2 Constraints ................................................................................................................. 28
2.5.3 RHC Framework ....................................................................................................... 29
2.5.4 Results ....................................................................................................................... 29
2.6 Summary ......................................................................................................................... 33
Chapter 3 MILP Receding Horizon Trajectory Planning ............................................................. 35
-
vii
3.1 Background and Overview .............................................................................................. 35
3.2 Problem Statement .......................................................................................................... 36
3.3 MILP Formulation ........................................................................................................... 37
3.3.1 Obstacle Avoidance ................................................................................................... 39
3.3.1.1 Conventional Obstacle Avoidance ...................................................................... 39
3.3.1.2 LOS-based Obstacle Avoidance .......................................................................... 40
3.3.2 Vehicle Dynamics ..................................................................................................... 43
3.3.3 RHMILP .................................................................................................................... 43
3.4 Results ............................................................................................................................. 44
3.5 Summary ......................................................................................................................... 48
Chapter 4 Robust MILP Receding Horizon Trajectory Planning ................................................. 49
4.1 Background and Overview .............................................................................................. 49
4.2 Problem Statement .......................................................................................................... 50
4.2.1 State Estimation ......................................................................................................... 51
4.2.1.1 Open Loop Prediction ......................................................................................... 51
4.2.1.2 Partially Closed-loop (PCL) Prediction .............................................................. 52
4.2.2 RHC Scheme ............................................................................................................. 53
4.3 Chance Constraints .......................................................................................................... 54
4.3.1 Single Linear Chance Constraints ............................................................................. 54
4.3.2 Sets of Linear Chance Constraints ............................................................................ 56
-
viii
4.3.2.1 Disjunctive Linear Constraints ............................................................................ 57
4.3.2.2 Conjunctive Linear Constraints ........................................................................... 58
4.4 MILP Formulation ........................................................................................................... 59
4.4.1 RHMILP .................................................................................................................... 59
4.4.2 Obstacle Avoidance ................................................................................................... 60
4.4.2.1 Conventional Obstacle Avoidance ...................................................................... 60
4.4.2.2 LOS-based Obstacle Avoidance .......................................................................... 62
4.4.2.3 Convex Polyhedral Obstacle ............................................................................... 64
4.5 Simulation Results ........................................................................................................... 67
4.6 Experimental Results ....................................................................................................... 73
4.6.1 Experimental Test-bed .............................................................................................. 73
4.6.2 Results ....................................................................................................................... 75
4.7 Summary ......................................................................................................................... 77
Chapter 5 Conclusions and Future Work ...................................................................................... 79
5.1 Summary ......................................................................................................................... 79
5.2 Future Work .................................................................................................................... 81
Bibliography ................................................................................................................................. 82
-
ix
List of Tables
Table 3.1 Simulation scenarios are designed to study the efficacy of the proposed algorithm. ..45
Table 4.1 Example values for the level of confidence versus the . ................56
Table 4.2 Simulation scenarios are designed to study the efficacy of the proposed algorithm. ..67
Table 4.3 The failure rate for different execution horizons is shown. As the execution
horizon increases, the failure rate also increases [3]. ...................................................70
Table 4.4 Two experiments are designed to study the efficacy of the proposed algorithm. ........75
-
x
List of Figures
Figure 2.1 The system output over the planning horizon is predicted and the calculated
control actions are obtained. Thereafter, the control actions are partially
implemented. This process is repeated until the goal is achieved [1]. .......................21
Figure 2.2 TheUGVsobjectiveistomovetothedesiredlocation(fire).TheUAVprovides
the localization data for the system. The UAV uses GPS for self-localization and
vision for localizing the UGV and the fire. The localization data is then
transferred to the UGV i.e., the control unit, through vehicle-to-vehicle (V2V)
communication. ..........................................................................................................25
Figure 2.3 The designed path for the UGV has to drive it from the initial starting point to
the target point (fire) while avoiding obstacles. .........................................................26
Figure 2.4 (a) The UGV reduces its distance with fire. The UAV is also constantly
decreasing its distance with the UGV and the fire. (b) As a result of the motion
of the UAV, the uncertainty associated with the system is decreasing. .....................31
Figure 2.5 The objective for the UGV is to reach to the fire while avoiding the circular
obstacle. Meanwhile, the UAV tries to increase the accuracy of the estimation
scheme by getting closer to the UGV and the fire while avoiding the no-fly zone. ..31
Figure 2.6 (a) As the UGV gets closer to the fire, its velocities in both directions approach
zero to maintain the distance between the UGV and the fire. (b) As the UAV gets
close to the state where the uncertainty is minimized, the velocity decreases to
zero so that the UAV stays at its optimal state. ..........................................................32
-
xi
Figure 3.1 The top left quarter of the original and enlarged obstacle. The first enlargement
enables the point mass assumption of the robot. The second enlargement is
related to discrete time planning [2]. ..........................................................................42
Figure 3.2 (a) The robot that follows the LOS-based obstacle avoidance trajectory performs
a detour motion around the corners of the obstacle thus decreasing the chance of
collision with an unforeseen obstacle located behind the main obstacle. (b)
Obstacle enlargement in the conventional obstacle avoidance method blocks the
robots path and results in the trajectory to go around the obstacles. (c) The
coordinates of the dynamic obstacle with respect to time is shown with different
line styles. Due to requiring bigger obstacle enlargement, the obstacle avoidance
method is not able to generate a feasible trajectory for the robot. On the other
side, the path generated by the LOS-based obstacle avoidance method is able to
tackle both dynamic and static obstacles effectively and reach to the goal
position [3]. ................................................................................................................47
Figure 4.1 is a single-variate normal random variable . When the variance is
fixed, is the necessary and sufficient condition for . ....................55
Figure 4.2 A two dimensional polyhedral obstacle with 5 faces (pentagon) is schematically
shown. The polyhedron is defined by where
represents the face [3]. ..........................................................................................65
Figure 4.3 A path planning problem is solved in three different cases: OL (green), PCL with
low quality measurement (red), and PCL with high quality measurement (grey).
In the OL approach, the uncertainty of the future robot position grows
unboundedly. In contrast, the PCL approach bounds the growth of uncertainty.
-
xii
(a) The grey line follows the intuitively optimal solution while the other two
diverge from the straight line. (b) The future robot position uncertainty for three
approaches is plotted to demonstrate that the PCL bounds the growth of
uncertainty. (c) The planned and executed paths for the OL are considerably
distinct. On the other side, efficient incorporation of anticipated future
measurements in the PCL results in having very similar planned and executed
paths [3]. .....................................................................................................................70
Figure 4.4 The effect of changing the level of confidence on the designed path is studied.
The paths generated for uncertain robot states are generally more conservative
than the path for the deterministic environment (black dashed line). As the level
of confidence is increased, the robot path between the obstacles moves further
away to avoid the obstacles. For the path can no longer go between
the obstacles and has to go around the obstacle [3]. ..................................................71
Figure 4.5 The designed trajectory is dependent upon the planning horizon as the planned
trajectories differ for different planning horizon values.............................................73
Figure 4.6 The interconnectivity of MATLAB, ROS, and Gazebo in the experimental
platform is illustrated. ROS transfers the localization request and localization
data from MATLAB to Gazebo and vice versa. It also executes the planned
motions and control commands ordered by MATLAB on Gazebo [3]. .....................74
Figure 4.7 The robot starting and final points are and , respectively
[3]. ..............................................................................................................................75
-
xiii
Figure 4.8 (a) In the beginning of the process, there are only two obstacles to be avoided.
(b) In the middle of the process, a third obstacle is added which occludes the
initial path [3]. ............................................................................................................76
Figure 4.9 (a) In the planning process, uncertainties are taken into account so that the
disturbances in the robot control input would not cause collision of the executed
trajectory with the obstacles. (b) The initial trajectory of the robot (before
appearance of the dynamic obstacle) is compared with the adjusted trajectory of
the robot (after appearance of the dynamic obstacle) and the executed trajectory.
(c) The input of the robot in x and y directions is plotted and compared to the
higher and lower bounds limits (red lines) [3]. ..........................................................77
-
xiv
List of Symbols
Position of robot in the direction
Position of robot in the direction
Position of robot in the direction
Time interval
Position vector of robot at time interval
Velocity vector of robot at time interval
Acceleration vector of robot at time interval
State vector of the robot at time interval
Covariance of the state of robot at time interval
Input vector of robot at time interval
System dynamics disturbance at time interval
Covariance of system dynamics disturbance at time interval
Normal random variable
Measurement vector at time interval
Measurement noise at time interval
Covariance of measurement noise at time interval
Nonlinear measurement function
Nonlinear system dynamics function
Probability of occurrence of event
State matrix in LTI systems
Input matrix in LTI systems
Disturbance matrix in LTI systems
-
xv
State matrix in linear measurement functions
Measurement noise matrix in linear measurement functions
Linearized state matrix in nonlinear systems
Linearized disturbance matrix in nonlinear systems
Linearized state matrix in nonlinear measurement functions
Linearized measurement noise matrix in nonlinear measurement
functions
Stage cost function in RHC
Final cost function in RHC
Desired state
Nonlinear constraint
Admissible set of inputs
Admissible set of states
Level of confidence
Existing obstacles at time interval
Planning horizon in RHC
Execution horizon in RHC
Identity matrix of size
Zero matrix of size
Sampling time
Logical conjunction
Logical disjunction
Robot diameter
-
xvi
A value between 0 and 1 that defines the position of interest in a line
A discrete set representing
Members of
Number of members of
, System input lower and upper bound vectors
, Lower and upper bound vectors for system input derivative
-
xvii
Acknowledgment
I, first of all, would like to express my gratitude to my supervisor, Dr. Homayoun Najjaran. It
would have been impossible to successfully finish my academic journey without your help and
support. Thank you for the continued trust you placed in me. I am thankful to the Natural
Sciences and Engineering Research Council of Canada (NSERC) for providing me with financial
support throughout the course of my research. A big word of appreciation goes to Nima, my
colleague and mentor, for all the fruitful discussions on research, ideology, and politics. Thanks
for teaching me how to think.
To my dearest friends, Reza, Mohammad, and Brie: I feel exceedingly privileged for having
you. I truly appreciate you being there for me and supporting me throughout.
To my Iranian friends, Behzad, Ehsan, Mo, and AR: I will be always remembering the joyful
moments of playing volley ball, gaming, barbequing, and swimming.
To my colleagues in the ACIS lab, Mohammad, Farzad, Nikolai, Hadi, and George: Thanks
for creating a cooperative, friendly environment in the lab by sharing all the discussions, laughs,
and support.
Last but not least, I am eternally grateful for the endless love, sacrifice, and support that I
have received form my family in my entire life. To them I dedicate this thesis.
-
xviii
Dedication
To my father, for his inspirational support,
to my mother, for her endless sacrifice,
and to my siblings, for their continued love.
-
1
Chapter 1
Introduction
This thesis is focused on the trajectory generation problem of robots in dynamic, cluttered
environments. We are aiming at tackling scenarios in which uncertainty in the robot states
plagues the system. The planned trajectories must be safe (safe in the sense of collision
avoidance), be robust against the uncertainties (robust in the sense that uncertainties affecting the
system do not bring the robot to unsafe situation), be optimal with respect to certain criteria,
while satisfying possible constraints on the system input, system states, or both.
1.1 Motivation
In recent years, unmanned vehicles have received increasing attention due to their various
military and civilian applications. Different varieties of unmanned vehicles are being widely
developed including but not limited to unmanned aerial vehicles (UAVs), unmanned ground
vehicles (UGVs), and unmanned surface vehicles (USVs). The obvious reduction of human
safety risk is one of the primary reasons for deploying these types of vehicles, as these vehicles
operate without requiring a human onboard. Unmanned vehicles can therefore widely contribute
in performing dull, dirty, and dangerous tasks such as search and rescue missions, household
chores, and surveillance of environments which are inaccessible to humans. That being said, as
the level of autonomy in the unmanned vehicles increases, the latter become more cost effective
and more capable of operating in harsh and remote environments. One famous example of a fully
-
2
autonomous (automated) unmanned vehicle is the Google driverless car [4]. This car is capable
of sensing its surrounding and navigating through the environment with no human intervention.
More recently, iRobot Ava 500 Tele-presence robot has been announced, and is another example
of an autonomous robot; designed to provide easy navigation in labs, houses, and offices
particularly for people who are not present on the site [5]. It needs to be clarified that the
emergence of autonomous robots should be perceived as a motivation for continuing the research
on autonomous robots in a larger scale as the existing algorithms can and must be improved in
every aspect. The latter has been the line of thought in this thesis.
In order to be fully autonomous, a vehicle or a robot requires a few key elements including
the ability to localize itself, map the environment, and plan the motions. Additionally, for
applications where a team of autonomous robots are employed, communication and task
allocation capabilities are added to the mentioned list. These functionalities should guarantee the
flexibility, robustness, efficiency, resilience, and safety of the overall system. The robotics
literature has been subject to continuing effort to increase the performance of autonomous
vehicles including but not limited to simultaneous localization and mapping (SLAM) [68], task
allocation between a team of robots [911], and motion planning [1214].
In this thesis, the problem of interest is robot motion planning. In the robotic literature,
motion planning, also known as trajectory generation and the piano movers problem, is
concerned with finding the control actions that will take a single robot or team of robots from an
initial state to the goal state [14]. The environment may contain static and dynamic obstacles that
the robot must avoid collision with. An obstacle with constant coordinates is called a static
obstacle. Dynamic obstacles are non-static obstacles. For example, while navigating through the
environment, other moving autonomous agents are considered dynamic obstacles and the robot
-
3
path must not collide with them. Furthermore, the available system information may be plagued
by different sources of uncertainty, including but not limited to process disturbances,
measurement noise, and dynamicity of the environment. Safe and robust motion planning
frameworks must be created to design trajectories that ensure the safety of the robots, account for
system uncertainties, and are optimal with respect to certain criteria (e.g., time or fuel or both).
In the remainder of this chapter, an overview of the motion planning literature is presented and
the thesis is outlined.
1.2 Literature Review
This section aims at presenting a brief overview of the existing motion planning algorithms in
the robotics literature. Motion planning algorithms can be categorized from different
perspectives. Motion planning algorithms have been developed by either the dynamics and
control community [13, 14] or robotics and artificial intelligence community [15, 16]. While the
former is more concerned with the dynamical behavior and state and control constraints, the
latter is focused on computational issues and developing frameworks that are capable of real-
time control [17]. Historically speaking, research on the motion planning problem was first
conducted for deterministic systems with no differential constraints while it was later expanded
to account for both uncertainties and differential constraints in the problem [17, 18]. We
categorize the motion algorithms to: (i) Non-RHC motion planning, and (ii) RHC-based motion
planning. It is worthwhile to mention that in this thesis, the phrases trajectory planning,
trajectorygeneration,andplanningareallequaltothephrasemotionplanningandwill be
used interchangeably. For more extensive background on motion planning algorithms, interested
readers are referred to the classic textbook written by Latombe [12] where many basic concepts
areintroduced.ThePlanningAlgorithmsbookbyLaValle[14] studies more recent techniques
-
4
and algorithms related to motion planning. The surveys by Goerzen et al. [17], Dadkhah and
Mettler [19], and Hoy et al. [20] are more recent reports on the existing motion planning
algorithms for deterministic and uncertain environments.
1.2.1 Non-RHC Motion Planning Methods
The very first motion planning algorithms were developed based on the optimal control principle
[21] and dynamic programming technique [22] and dealt with holonomic systems in obstacle
free environments [23]. The first methods to tackle the motion planning problem in the presence
of obstacles were based on graph search algorithms such as Dijkstras [24, 25] and A* [26]
algorithms. The exact and approximate cell decomposition approaches are famous examples
where the obstacle-free environment (the configuration space) is divided into a number of cells
and the goal is to find and connect a sequence of neighboring cells and build a path for the robot
[27, 28]. The so-called roadmap approaches (e.g., visibility graphs [29] and Voronoi diagrams
[30]) are other graph searching methods. In these methods, a line connecting two cells is a
feasible path if it is located inside the configuration space. Thereafter, a network of all feasible
paths is constructed and the path planning problem is solved using a graph search algorithm such
asDijkstras algorithm to connect the initial and final configuration with the shortest line through
a set of feasible paths. These methods do not consider the differential constraints or
nonlinearities of the system in their solution. Thus, although a collision-free path for the robot
can be constructed using these approaches, the robot might fail to follow the designed path due
to its nonlinear dynamics or velocity constraints.
Potential field methods are another group of techniques which solve the motion planning
problem through a vector minimization [3133]. A vector is assigned to each point in the
workspace. The vector assigned to the desired state has the minimum value and functions as an
-
5
attractive force while the obstacles function as a repulsive force. The vector value for each point
is calculated based on these attractive and repulsive forces. Since the goal owns the minimum
potential, minimizing the vector value will result in the robot traveling from its initial
configuration to the goal while trying to avoid the obstacles (due to repulsive forces). The
original approach proposed by Khatib [31], was not capable of dealing with differential
constraints of the system. More recent field techniques can produce smooth and dynamically
feasible trajectories and take into account the mentioned constraints [32, 33]; however, they have
two main disadvantages. First, robot navigation can fail due to the robot getting trapped in the
local minima. Second, the obstacle avoidance constraints are not modeled as hard constraints and
might be violated.
A group of more recent algorithms that are powerful tools for planning in high-dimensional
spaces are sampling-based algorithms [34, 35]. Suitable for systems with differential and non-
holonimic constraints, these approaches are based on generating randomized robot
configurations in the work space. The idea is very much like roadmaps or cell decompositions
(combinatorial planning techniques in general) in the sense that two configurations can be
connected to form a feasible path if the connecting line is located outside of the obstacle region.
However, the difference between the sampling-based algorithms and the combinatorial
approaches is that the former generates random samples based on dynamics and constraints of
the system i.e., the samples are generated in the free state-space. Examples of famous sampling-
based algorithms are tree-based planners (e.g., rapidly-exploring random trees (RRTs)) [36] and
probabilistic roadmap techniques (PRMs) [37].
-
6
1.2.2 RHC-based Motion Planning
Mathematical programming methods are techniques that solve the trajectory planning problem
through a numerical optimization framework and thus are also called trajectory optimization
methods [17, 38]. The early trajectory optimization algorithms were developed for navigation of
robots through obstacle free environments using optimal control and dynamic programming [23].
While the earlier groups of these approaches were only tackling the motion planning problem in
deterministic environments, the next generations became capable of dealing with uncertain
environments as well [39]. However, they were computationally expensive and could not readily
handle hard constraints [40]. Receding horizon control (RHC) algorithms are mathematical
programming approaches that aim at reducing the complexity of the motion planning problem
and are able to handle system constraints and uncertainties in a systematic way [41]. RHC, also
known as model predictive control (MPC), is a suboptimal control scheme that is based on
iterative constrained optimization over a finite horizon. The RHC algorithm is as follows. At
each iteration, a cost function generated based on the system model is optimized over a so-called
planning horizon and a set of optimal system inputs is obtained. Thereafter, the optimal control
input set is partially executed over a shorter horizon called execution horizon and the new system
states are measured. The operation is repeated until the goal is achieved. In the optimization step,
the cost function is calculated based on the predicted future system states and control inputs. In
doing so, the system model is used to predict the future system states thus ensuring that the
offered solution meets the system constraints e.g., differential constraints and nonlinearities of
the system. RHC is beneficial in that it can be utilized for multi-system control, handle system
constraints and uncertainties, and generate solutions that allow the systems to work closer to
their constraints, thus offering good performance [42]. The interested reader is referred to the
-
7
studies on the structure, stability, and optimality of RHC by Goodwin et al. [41], Mayne et al.
[43], and Morari and Lee [44].
RHCs were originally used in process industries to control chemical plants [45]. Jadbabaie
[46] was the first to utilize RHCs as a framework for motion planning of nonlinear systems in
obstacle-free environments. RHCs were later used by Singh and Fuller as a control scheme for
robots to follow a (pre-determined) trajectory around obstacles in a non-convex region [47].
Originally, deterministic systems were of interest [48]; however, the RHC has been extended to
stochastic systems as well [40]. The latter has been addressed by assuming either probabilistic
uncertainties [40] or set-bounded uncertainties [49]. In this thesis, the stochastic systems with
probabilistic uncertainties are of interest. In such systems two important features are noticeable.
First, the system states are not exactly known and have to be estimated. Second, since only an
estimation of the states is available, instead of deterministic constraints, chance constraints are
utilized i.e., the probability of constraint violation has to be below a certain value [50]. Yun and
Bitmead [51] were the first to incorporate uncertainties, and thus state estimation, in the
stochastic RHC for a linear Gaussian system. In doing so, if both the initial state estimation and
uncertainties are of Gaussian type, the future system states can also be modeled as Gaussian
distributions. In their work, however, future information was ignored which resulted in an open
loop future state estimation and a growing predicted state covariance which contributes to overly
conservative solutions. To overcome this issue, they introduced a closed-loop covariance i.e., the
one-step ahead covariance is considered as the predicted future state covariance for all time steps
[52]. Blackmore et al. adopted the stochastic RHC framework presented in [52] to develop a
motion planning scheme for robots with uncertain positions in the presence of obstacles [53]. In
another work, Blackmore at al. [54] developed a framework that also assumed the initial system
-
8
states and the sources of the uncertainties to be Gaussian. However, this work predicted the
distribution of the future states by using sampled particles. Du Toit and Burdick [55] extended
the state estimation approach presented by Yun and Bitmead ([52]) by incorporating the
anticipated future information into the estimation algorithm. This state estimation algorithm,
named partially closed-loop state estimation, assumes the most likely future measurements to
occur. In doing so, the fact that future measurements are going to happen is appreciated while the
least information gain is introduced to the controller [40]. As a result, the predicted state
covariance is bounded and does not grow substantially over time which contributes to a more
useful solution.
The general RHC motion planning scheme is more deeply structured as follows. Future
system states are predicted based on initial system states and (to be determined) system inputs. A
nonlinear cost function, which is the core of the planning problem, is optimized over a finite
horizon. The optimization is subject to linear and nonlinear constraints that guarantee the
feasibility of the planned trajectory in terms of both collision avoidance and kinodynamic
constraints. However, under certain assumptions i.e., robots with linear dynamics and convex
polyhedral obstacles, the RHC scheme for trajectory planning can be modeled as receding
horizon mixed integer linear programming (RHMILP) [56]. In MILP, discrete logic and integer
variables are included in a continuous linear optimization problem. In doing so, the kinodynamic
constraints are modeled as continuous linear constraints while logical constraints such as
collision and obstacle avoidance are formulated using integer variables. For more information on
MILP and its applications, the interested reader is referred to reference [57]. The formulation of
motion planning schemes using RHMILP was first reported by Schouwenaars et al. [56] where
the problem was solved for deterministic environments and in the presence of rectangular
-
9
obstacles. In the original RHMILP formulation, safety, in the sense of collision avoidance, is
guaranteed by enforcing the robot outside of the obstacle coordination at all discrete planning
time steps. Later, reference [58] introduced a further safety constraint for RHMILP motion
planning i.e., ensuring that at all time steps, a path to a state that is naturally collision free exists.
The RHMILP motion planning framework has also been extended to problems with uncertain
environments where the uncertainties are modeled as set-bounded variables [49, 59].
RHMILP handles hard constraints e.g., obstacle and collision avoidance, systematically
while offering a more computationally viable framework compared to the general nonlinear
programming approaches. Thanks to the emergence of faster computers and powerful
optimization software such as CPLEX [60], RHMILP is considered to be a suitable framework
for real-time trajectory planning [61, 62].
1.3 Statement of Contributions
In this thesis we build upon and introduce new features to the existing RHMILP literature. The
thesis has two contributions as follows.
We formulate a new state estimation algorithm for planning under uncertainty in
RHMILP. As was mentioned above, the current RHMILP frameworks either assume
deterministic environments or characterize the uncertainty as a set-bounded model. In this
thesis, however, the uncertainty is assumed to be of a probabilistic nature. The partially
closed-loop (PCL) state estimation strategy is utilized to predict the future system states.
In doing so, the future system states are estimated based on the anticipated future state
measurements that can in turn help to reduce the uncertainty of the future states.
We introduce a new obstacle avoidance constraint for motion planning of robots with
deterministic and uncertain states in RHMILP framework. The obstacle avoidance
-
10
constraint is designed to secure a line connecting each pair of sequential robot positions
inside the configuration space (outside the obstacles) at all discrete time, analogous to
maintaining line-of-sight (LOS) between each pair of sequential positions of the robot. In
the conventional RHMILP obstacle avoidance formulation, obstacle avoidance is
guaranteed by locating the robot outside of the obstacles at all discrete time steps. LOS-
based obstacle avoidance is advantageous in that it can typically provide the same level of
safety at time steps larger than those of the conventional approaches; hence creating
trajectories which are more effective at both planning and execution stages.
1.4 Thesis Overview
The thesis is organized as follows. Chapter 2 formulates the general receding horizon scheme
that can be used for trajectory planning of single and multiple robots. The problem is formulated
as nonlinear programming. The PCL is utilized for future state prediction. The developed scheme
is examined in a case study i.e., cooperative motion planning of unmanned vehicles for wildfire
suppression. Numerical simulations are carried out to study the performance of the proposed
scheme. In Chapter 3, the RHMILP framework for motion planning of robots in deterministic
environments is studied. The robots are assumed to have linear dynamics and the obstacles are
convex polyhedral. LOS-based obstacle avoidance constraint is formulated and is incorporated
into the RHMILP framework. Numerical simulations have been carried out to study the efficacy
of the proposed algorithms in dynamic, cluttered environments with narrow passages. Chapter 4
extends the RHMILP scheme developed in Chapter 3 to environments where the robot state is
uncertain. The PCL strategy is utilized for future state prediction. The safety constraints
formulated in Chapter 3, including LOS-based obstacle avoidance, are extended for uncertain
robot states using a probabilistic framework (chance constraints) to ensure the robustness of the
-
11
designed trajectory against the uncertainties. We then show that despite extending the algorithm
to environments with uncertain robot states, it can still be modeled in an RHMILP framework.
Numerical simulations and experimental studies are carried out to investigate the applicability of
the proposed framework in dynamic, cluttered environments with uncertain robot states. Chapter
5 then concludes the thesis and briefly points out some intended future directions of the research.
-
12
Chapter 2
Receding Horizon Trajectory Planning
2.1 Background and Overview
This chapter is concerned with the development of a general motion planning framework using
RHC, general in the sense that system and measurement equations are nonlinear and obstacles
are arbitrarily shaped. We are interested to design a (motion) planning and control scheme that
drives the vehicles optimally in the environment (optimal in the sense that the performance of the
system is optimized with respect to some attributes) and is probing (minimizes system
uncertainty). The generated trajectories must assure the safety of the system (safety in the sense
of collision avoidance) even in the presence of uncertainties i.e., be robust against uncertainties.
The presented framework can be used for single or multiple robot motion planning tasks.
RHC is employed as the trajectory planning and control scheme. RHC is an optimization-
based framework which is able to handle constraints and uncertainties explicitly [41]. In RHC, at
each sampling time, an open-loop optimal control problem (balancing a tradeoff between the
control action and system states) is solved over a finite horizon. Then, the control action
sequence is applied to the system for a period after which the optimal control problem is re-
calculated based upon the new states of the system. The ability to explicitly tackle the constraints
has made the RHC an important method for motion planning, see for example [6365].
-
13
Due to the existence of uncertainties in the problem, the current and future system states
cannot be exactly determined. The system states are associated with uncertainties mainly due to
the following reasons:
The initial state of the system is assumed to be reported by a state estimator e.g., Kalman
filter.
The system model is not exactly known. This uncertainty arises due to system model
linearization or modeling errors.
Process disturbances (e.g., wind forces) affect the system motion.
The measurement data is corrupted by measurement noise.
The formulation of the RHC framework includes prediction and filtering algorithms. In
particular, the partially closed-loop (PCL) strategy proposed by Du Toit [40] is of interest as the
prediction algorithm. In the PCL state estimation method, at each future time step, the most
possible measurement is assumed to occur.
This chapter is organized as follows. The problem of interest and the system and
measurement equations are defined in Section 2.2. In Section 2.3, the state estimation algorithms
(filtering and prediction) are formulated. The RHC scheme is developed in Section 2.4 and
consists of two steps: planning and execution. A novel case study for motion planning
(automated firefighting) is presented in Section 2.5. The presented case study is novel in the
sensethat theproposedframework, to theauthorsknowledge,hasnotbeenutilizedbeforefor
an automated firefighting problem. The presented scheme is formulated to handle various
sources of uncertainty as well as environment constraints by incorporating them to the
conventional nonlinear model predictive control. Finally, the concluding remarks are
summarized in Section 2.6.
-
14
2.2 Problem Statement
This section presents the problem setup for motion planning of robots. The dynamics of the
vehicles (robots) in discrete time is described as
(2.1)
where and are the system state and input vectors at time interval , respectively. The
dimension of the system state and system input vectors are assumed to be and . It is
assumed that the initial state of the system, , is given a priori and has a Gaussian distribution
. The robots motion is subject to process disturbance and system model
uncertainty which are modeled accumulatively as a Gaussian white noise and is distributed
according to . The state measurement mapping for the system is described as
(2.2)
Here, is the vector representing the system output and is Gaussian white
noise that represents the measurement noise. We aim at developing a motion planning and
control framework: deliver robots from their starting configuration to their goal while avoiding
obstacles.
2.3 State Estimation
Initial uncertainty of the robot states, as well as existence of process disturbances and motion
model uncertainties cause the system states to be continuously associated with uncertainty. These
uncertainties can contribute to deviation of the robot from its planned trajectory. Therefore,
failure to account for system uncertainties might decrease the safety of the planned trajectory,
safety in the sense of collision avoidance. The first step towards handling uncertainties is to
-
15
determine their effect on the system states. In doing so, the system states are continuously
estimated using the available measurements, the system equations and the uncertainties as a
probabilistic distribution. The state estimation framework in this thesis is divided into two parts:
filtering and prediction. In filtering, the current system states are estimated using the past and
current measurements. In prediction, which is of interest to planning, the uncertainty of the
future states is assessed (and future states are predicted) using the currently available
information.
2.3.1 Filtering (Recursive Bayesian Estimation)
The recursive Bayesian estimation framework is utilized for filtering [66]. In doing so, the
mathematical system model and measurements are fused to estimate a probability density
function for the system states i.e., | . In order for the estimation process to be
formulated recursively, it is assumed that the states are a Markov process. This means that
knowing the current state estimation and the system input, the future states can be predicted
independently of the earlier states. More precisely, earlier system data (states, measurements, and
system input) do not add any additional information to the prediction of the future states [66].
The recursive Bayesian state estimation framework contains two steps: prediction and update. In
the prediction step, the system dynamics model is used to calculate the probability of the current
system states. In the update step, the latest taken system measurements are incorporated into the
estimation.
It was noted by Kalman [67] that incorporating the measurements up to the current time (and
the system mathematical model) in the estimation framework leads to an optimal estimation of
the system state. In Kalmansformulation, thisestimationisobtained by solving a least square
error problem where the square of the state estimation error (the variance) is minimized [66].
-
16
2.3.1.1 Linear Gaussian System: Kalman Filter
In general, a closed-form formulation for optimal state estimation does not exist. However, when
system dynamics and measurement equations are linear and the uncertainties are modeled as
Gaussian distributions, a closed-form formulation for state estimation is developed and is called
the Kalman filter. The system dynamics and measurement equations are modeled linearly as
(2.3)
(2.4)
where are known matrices. Knowing the a priori state estimation |
( | | ), measurements at the current time , and the measurement and system motion
models, the Kalman filter is formulated to estimate the current system state |
( | | ) in a two-step process as follows. The prediction step is:
{ | |
| |
(2.5)
and the update step is:
{
| ( |
)
| | ( | )
| |
(2.6)
where is called the Kalman gain.
2.3.1.2 Nonlinear Gaussian Systems: Extended Kalman Filter
The Kalman filter, modeled in Section 2.3.1.1, is not applicable to systems with nonlinear
equations or non-Gaussian uncertainties. In the case of nonlinear dynamic and measurement
-
17
equations, the states do not remain Gaussian after passing through the system equations. Hence,
the optimal filter (the solution to the least squares problem) has to be developed for non-
Gaussian distributions thus resulting to higher computational burden [68]. The Extended Kalman
filter (EKF) is a non-optimal approach for state estimation of systems with nonlinear equations.
This is an approximation approach since the nonlinear equations are linearized and a Kalman
filter is derived for the linearized system. For a system described by Eq. (2.1) and (2.2), the
prediction step of the EKF is given by:
{ |
| |
(2.7)
and the update step is:
{
| ( |
)
| | ( ( | ))
| ( ) |
(2.8)
In Eq. (2.7) and (2.8),
| | ,
| | ,
| | , and
| | are
the linearized approximations of the system and measurement equations which are derived by
first-order Taylor series expansion. These matrices are analogous to ,
respectively. For a more general formulation of EKF, where the process disturbances and
measurement noise are non-zero mean Gaussian, the interested reader is referred to [40].
2.3.2 Prediction
In Section 2.3.1, the Kalman filter and EKF were introduced as the two most common filtering
approaches for linear and nonlinear systems, respectively. In planning, we also need to predict
the future system states. This is a straightforward task to achieve in deterministic environments:
-
18
the exact value of the future system states is predicted using system dynamics. In uncertain
environments, however, the future system states cannot be accurately predicted solely by system
dynamic equations. This happens due to the fact that different sources of uncertainty do not
allow the system to evolve exactly as planned. Instead, an estimation of the future system states
has to be predicted accounting for uncertainties (called prediction). Three prediction algorithms
are presented in the rest of this section.
2.3.2.1 Open Loop Prediction
In open loop prediction algorithms, measurements up to the current time are utilized. Future
measurements are ignored since there is no future information available currently [51, 53]. In
open loop prediction, the system states are predicted in future time for a given control sequence
using system dynamics. The process disturbances affect the predicted uncertainty of the system
states. For linear and nonlinear systems, the open loop prediction algorithms are the same as the
prediction steps of the Kalman filter and EKF, respectively. In doing so, given a priori state
estimation | | | the system dynamics are utilized to predict the mean and
covariance of the future system state. For instance, in a linear system,
{ | |
| |
(2.9)
where | | | is an estimation of the future state of the system.
The disadvantage of this approach is that the uncertainty of future system states grows
unbound as a result of considering process disturbances and ignoring future information. We will
elaborate in Sections 2.4.1.1 and 4.3 (the latter specifically) why this property is a disadvantage
for the motion planning scheme.
-
19
2.3.2.2 Approximately Closed-loop Prediction
As mentioned, the use of open loop prediction causes the future predicted covariance to be
unbounded. Yan and Bitmead [52] introduced a new state estimation approach that bounds the
future system state uncertainties. In their approach, the one-step-ahead covariance (the
covariance at the next time step) is artificially set as the future predicted state covariance.
Similarly to the open loop state estimation approach, the mean of future system states is
predicted using system dynamics. This approach prevents the uncertainty from growing but
ignores future expected measurements.
2.3.2.3 Partially Closed-loop Prediction
In this thesis, the PCL prediction strategy proposed by Du Toit [40] is utilized. The key idea
behind this approach is that though not currently available, future measurements will be obtained
in the future. Thus, the effect of expected future information should be somehow incorporated in
the prediction process. From the context of the recursive Bayesian frameworks (Section 2.3.1) it
is inferred that incorporating the measurements has two effects on the estimated system state: (i)
the value of the measurement affects the predicted state mean, and (ii) the measurement noise
characteristics (noise covariance) affects both the mean and the uncertainty (covariance) of the
state. It is then noted that the uncertainty of the predicted state is not dependent on the actual
value of the measurement. Thus, the effect of the future measurement can be incorporated in the
state prediction by updating the system uncertainty using the noise covariance. In the PCL
strategy, the most probable measurement is assumed to occur. By this assumption, the mean of
the predicted state is not changed but the uncertainty is corrected accounting for the fact that
future measurements are expected. The latter can be observed from Eq. (2.11) where the mean of
the system state (second line in Eq. (2.11)) stays unchanged and is not affected by incorporating
-
20
the uncertainty of the future measurement. It is proved in [40] that by assuming the most likely
measurement, no information is artificially introduced to the prediction problem. The Kalman
filter and the EKF can be modified to formulate the prediction algorithm for linear and nonlinear
systems, respectively. Thus in this thesis, the prediction algorithm is called the modified Kalman
predictor (MKP). The MKP for nonlinear system equations and white Gaussian noise is
formulated as follows. The prediction step is:
{ |
| |
(2.10)
and the update step is:
{
| ( |
)
| |
| ( ) |
(2.11)
As expected, the most probable measurement assumption does not affect the state mean but
results in updating and decreasing the state uncertainty.
2.4 RHC Algorithm
In this subsection, a Receding Horizon Control (RHC) algorithm is developed as the motion
planning scheme. RHC, also known as Model Predictive Control (MPC), has a flexible
architecture in which state and input constraints as well as system uncertainties can be handled
systematically [69]. In RHC, the control problem, which is modeled as an optimization problem,
is iteratively solved (planning). The calculated control input set is then partially executed
(execution) and the new system states are measured. The procedure is repeated until the goal
state is achieved. The RHC framework is schematically shown in Figure 2.1.
-
21
Figure 2.1: The system output over the planning horizon is predicted and the calculated control actions are obtained. Thereafter, the control actions are partially implemented. This process is repeated until the goal is achieved [1].
2.4.1 Planning
In the planning stage, the control problem is formulated as an optimization problem over the next
horizons called the planning horizon. The solution to this problem is a set of control actions
that optimize the system performance with respect to some attributes while satisfying problem
constraints. Knowing the a priori state estimation | | | the motion planning is
expressed as an optimization problem: Find the sequence of optimal control inputs (
and to ) that minimizes an additive cost function and satisfies system dynamics and
problem constraints.
( ( ( | ))
( | )) (2.12)
subject to
( | ) (2.13)
k+1 k+2 k+M k+N
u(k+1)
System input
Prediction horizon
Control horizon
Predicted output
Reference outputPast Future
-
22
The constraints will be elaborated in Section 2.4.1.1. The final desired state is shown by . The
functions and are called the stage cost and the final cost, respectively. The latter, in order to
guarantee globally optimal solutions, must be the exact cost-to-go from the final state in the
planning horizon ( | ) to the final desired state [70]. It is impossible, however, to calculate
the exact cost-to-go especially where the environment is not fully characterized at the moment.
Moreover, in order to find the ideal cost-to-go, a fixed horizon control problem has to be solved
for each planning iteration that contributes to increasing the computational cost [65]. The final
cost is thus usually calculated heuristically. As a result, it is expected that the offered solution by
the RHC algorithm might not be a globally optimal solution and the RHC is a sub-optimal
framework. There is no specific formulation for the stage and final cost functions as they are
dependent on the control objectives. They, however, are similar in that they indicate how far the
future system states will be from the desired state. Throughout this thesis, different cost
functions, designed for different applications and scenarios, will be introduced.
2.4.1.1 Constraints
In general the constraints to the optimization problem are modeled as nonlinear equality
functions ( | ) . This section will study the optimization constraints in more
detail. However, the exact constraint formulations are postponed to Section 2.5 and Chapter 3
and Chapter 4 where the problem of interest is defined more precisely. There are basically three
constraints in the (motion) planning problem: (i) constraints on the system state and the system
input that is aimed to account for the system dynamics equation, (ii) constraints on the system
input, and (iii) constraints on the system states. These constraints are formulated respectively in
Eq. (2.14) to Eq. (2.16).
| | (2.14)
-
23
(2.15)
( | ) (2.16)
Here, and are the set of admissible inputs and states, respectively. In the robot motion
planning problem, the latter two constraints represent actuator limitations, kinodynamic
constraints (e.g., velocity and acceleration bounds, limited turn rate, etc.), collision avoidance,
and other required constraints (dependent on the application). The commonly used obstacle
avoidance constraint in the literature for motion planning of robots is called the conventional
obstacle avoidance method (see Definition 1).
Definition 1: At time step , the obstacle avoidance of a robot positioned at [ ] is
guaranteed, if and only if where is the obstacle for the robot at time step .
It was mentioned earlier that due to uncertainties in the problem, the system states are not
deterministically known and thus are estimated. Since the system states are represented by
probabilistic values with unbounded distributions, it is impossible to guarantee that a state
constraint is satisfied for all possible realizations of a state. Instead, chance constraints are
imposed on the system state: the probability of constraint violation is assured to be below a
certain level. In the chance constraint formulation on system states in Eq. (2.16), denotes
the probability of occurrence of event and is the level of confidence. The (motion) planning
constraints are nonlinear in general. However, in certain conditions (e.g., linear system
equations, polyhedral obstacles, and Gaussian uncertainties), it is possible to model the
constraints as linear inequalities on the system states and inputs. This will be further studied
in Chapter 3 and Chapter 4 in the concept of RHMILP.
-
24
2.4.2 Execution
In the RHC algorithm, after solving the motion planning problem for the next horizons and
obtaining the set of inputs, , the system executes the first control actions to the system.
In order to estimate the newly obtained system states after the control actions have been applied,
a filtering algorithm (e.g., a Kalman filter or an EKF) is utilized (Section 2.3.1). Knowing the a
priori state estimation | ( | | ), measurements at the current time , and the
measurement and motion models (Eq. (2.1) and Eq. (2.2)), the system state | is estimated
and has a Gaussian distribution | ( | | ). The planning and execution
stages are repeatedly performed until the goal state is achieved.
2.5 Motion Planning for Automated Firefighting: A Case Study
In this section, the general RHC presented in this chapter is adopted to tackle the motion
planning of an automated wildfire suppression framework. As a matter of fact, the latter is
investigated as a case study for the broader problem of cooperating aerial and ground vehicles.
The proposed framework consists of an unmanned aerial vehicle (UAV) and an unmanned
ground vehicle (UGV) cooperating to detect, localize, and reach a fire. In this automated
cooperative setting, the UGV moves towards the fire. Meanwhile, the UAV gathers and reports
the localization data (the position of the fire, the position of the UGV, and the position of the
UAV) to the system. A schema of this automated firefighting system is shown in Figure 2.2. The
system dynamics and measurements are plagued with sources of uncertainty. Ground and aerial
obstacles exist in the environment. The problem of interest is to develop a motion planning and
control scheme for this automated system that: (i) drives the UGV to the desired target in
minimum time and with minimum fuel consumption (Figure 2.3), (ii) controls the UAV so that
-
25
the data uncertainty is minimized, and (iii) guarantees the safety of the system (safety in the
sense of collision and hazard avoidance).
Figure 2.2: TheUGVsobjectiveisto move to the desired location (fire). The UAV provides the localization data for the system. The UAV uses GPS for self-localization and vision for localizing the UGV and the fire. The
localization data is then transferred to the UGV i.e., the control unit, through vehicle-to-vehicle (V2V)
communication.
2.5.1 System Modeling
The UGV and target motion are in the plane and the UAV moves in 3D space. The rotational
motions of the UAV are assumed to be small, thus negligible. The motion models of the UAV
and the UGV are approximated by double integrators. The UGV motion model in discrete time is
described as
*
+ [
] *
+ [
] (2.17)
here, [ ], [ ], and [ ] denote
the UGV position, velocity, and acceleration at time , respectively. Sampling time is shown
-
26
by and and are identity and zero matrices of size . The process disturbance is shown
with .
Figure 2.3: The designed path for the UGV has to drive it from the initial starting point to the target point (fire) while avoiding obstacles.
The UAV motion model is expressed as
*
+ [
] *
+ [
] (2.18)
where [ ] and [ ] denote the
position and the velocity of the UAV respectively. The acceleration of the UAV is denoted by
[ ]. The process disturbance is shown with . The target
(fire) motion is assumed to be a random walk and is expressed as
[ ] [ ][ ] (2.19)
where [ ]is the targets position vector at time and is a white
Gaussian noise used to model the random walk motion. We define [ ],
-
27
[ ], and [ ]. The input vector to the system at time is
represented by and the system state at time is as follows:
[ ] (2.20)
The overall system model that formulates the dynamics of the UGV and the UAV can then be
written as in Eq. (2.21).
(2.21)
In the above equation, is the process disturbance and is assumed to be Gaussian white noise,
, where is the covariance matrix at time .
The measurement function calculates localization values that are reported by the UAV
measurement system and is formulated in Eq. (2.22)
( ) (2.22)
where is Gaussian white noise representing the measurement noise and is the
covariance matrix. The measurement function is assumed to be a linear function and is modeled
in Eq. (2.23).
[ ] (2.23)
The UAV measurement system provides the localization data for the UGV, the UAV, and the
target (fire). The measurement noise covariance is considered to be a function of the
distances between the UAV and the UGV ( , the UAV and the ground , and the UAV
and the fire .
-
28
{
(2.24)
In the Eq. (2.24), is the Euclidean norm function. The measurement noise covariance is more
precisely expressed as where is a constant
matrix.
2.5.2 Constraints
System constraints in the defined scenario are obstacle avoidance, no-fly zone avoidance, and
bounded velocities and accelerations. The obstacles and the no-fly zone area are assumed to be
circles and cylinders, respectively. The obstacle avoidance constraint is formulated as
( )
(2.25)
where [ ] represents a circle (obstacle) with the center [ ] and the radius . The
robot position in 3D space is shown with [ ]. The no-fly zone avoidance constraint is
formulated in Eq. (2.26)
{
( )
(2.26)
where [ ] represents a cylinder (no-fly zone) with the cross section
[ ] and the height . The (obstacle and no-fly zone) avoidance constraints are
incorporated in the RHC framework through state constraints. The bounded velocity and
acceleration constraints are expressed as following:
,
(2.27)
-
29
where [ ] and [ ] are vectors that define the velocity and acceleration
bounds, respectively. The velocity and acceleration bounds are incorporated in the RHC through
state constraints and input constraints, respectively.
2.5.3 RHC Framework
The stage cost and the cost-to-go functions are formulated in Eq. (2.28) and Eq.(2.29),
respectively
( | ) | (2.28)
( | | ) ( | ) ( | ) (2.29)
where , , , and are non-negative scalars. In Eq. (2.29), ( | ) is an index that
represents the system uncertainty at the end of the planning horizon and is included in the cost
function to minimize the system uncertainty. As it can be seen from Eq. (2.28) and Eq. (2.29),
the stage and cost functions are specifically chosen to meet the design criteria i.e., determining
the sets of inputs that would drive the system from the initial configuration to the goal state with
a time-fuel optimal trajectory while increasing the accuracy of the state estimation.
2.5.4 Results
The developed MPC scheme was examined in a simulated automated firefighting scenario. The
UGV, UAV and fire initial positions were assumed to be [ ], [ ], and
[ ], respectively. There was a ground obstacle [ ] in the
environment. The no-fly zone for the UAV was a cylinder [ ] which was
considered to assure the safety of the UAV during its operation. Furthermore, in order to ensure
the safety of the UAV in terms of collision avoidance with ground objects, the UAV had to
always maintain its altitude above . The covariance matrix for the process
-
30
disturbance was assumed to be . The Gaussian
white noise for the fire motion was modeled as (* + *
+). The
constant matrix that is used to construct the measurement noise covariance was chosen to be
. The bounded accelerations and
velocities for the UAV in the plane were [ ] [
] and [ ]
[
], respectively. In the direction, the bounded accelerations and velocities for the
UAV were [ ] [
] and [ ] [
], respectively.
Furthermore, the UGV accelerations and velocities were limited to
[ ] [
] and [ ] [
], respectively. Finally, the
parameters , , , and were chosen to be 1, 1, 10, and , respectively.
The formulated MPC scheme ( , ) was employed to solve the motion planning
and control problem i.e., drive the UGV to the fire, minimize the system uncertainty, and satisfy
the constraints and design criteria. The results of this simulation are shown in Figure 2.4 to
Figure 2.6. The Euclidean distances between the UGV, the UAV, and the fire are shown in
Figure 2.4(a). In Figure 2.4(b), the system, the UGV position, and the fire position uncertainties
versus time are plotted. Figure 2.4(a) demonstrates that the UGV is decreasing its distance with
fire. Meanwhile, in order to decrease the system uncertainty, the UAV is getting closer to the
UGV and the fire. These results are consistent with Figure 2.4(b) where the uncertainties of the
system, the position of the UGV, and the position of the fire are decreasing. It can be noticed that
the minimum system uncertainty in Figure 2.4(b) happens at which is the same
time that the UAV has minimized its distance with the UGV and the fire in Figure 2.4(a).
-
31
(a)
(b)
Figure 2.4: (a) The UGV reduces its distance with fire. The UAV is also constantly decreasing its distance with the UGV and the fire. (b) As a result of the motion of the UAV, the uncertainty associated with the system is
decreasing.
(a)
(b)
Figure 2.5: The objective for the UGV is to reach to the fire while avoiding the circular obstacle. Meanwhile, the UAV tries to increase the accuracy of the estimation scheme by getting closer to the UGV and the fire while
avoiding the no-fly zone.
It needs to be mentioned that the final uncertainty in Figure 2.4(b) is so-called minimum in
the sense that the solution is resulted from an optimization problem. In general, however, the
2 4 6 8 10 12 14 16 180
5
10
15
20
25
30
t [s]
Dis
tan
ce
[m
]
UGV - Fire distance
UAV - Fire distance
UAV - UGV distance
0 2 4 6 8 10 12 14 16 1810
-2
10-1
100
101
t [s]
Un
ce
rta
inty
in
de
x (
Tra
ce
(
))
UGV position uncertainty
Fire position uncertainty
System uncertainty
Minimumsystemuncertainty
0 5 10 15 20 25
0
5
10
15
20
25
x [m]
y [
m]
UAV executed 2D path
UGV executed path
Fire (target)
Groundobstacle
UAVstartingpoint
UGVstartingpoint
No-flyzone
0 2 4 6 8 10 12 14 16 189
10
11
12
13
14
15
t [s]
z [
m]
UAV executed altitude
Lower bound for UAV altitude
-
32
final uncertainty is not guaranteed to be globally minimum as the RHC is a sub-optimal control
scheme.
The executed paths of the UGV and the UAV in the presence of the ground obstacle and the
no-fly zone are shown in Figure 2.5. In Figure 2.5(b) the UAV altitude is decreasing; a behavior
that is expected in order to increase the state estimation accuracy. However, the UAV altitude
has a lower bound limit of . It is also notable that the executed paths for both the UGV and
the UAV are robust against system uncertainties while avoiding their corresponding obstacle and
no-fly zone.
(a)
(b)
Figure 2.6: (a) As the UGV gets closer to the fire, its velocities in both directions approach zero to maintain the distance between the UGV and the fire. (b) As the UAV gets close to the state where the uncertainty is minimized,
the velocity decreases to zero so that the UAV stays at its optimal state.
The UGV and the UAV velocities in the plane are plotted in Figure 2.6(a) and
Figure 2.6(b), respectively. The velocities satisfy the bound limit constraints. As can be observed
from Figure 2.6(a), as the UGV approaches the fire, its velocity decreases so that it stays as close
0 2 4 6 8 10 12 14 16 18
-3
-2
-1
0
1
2
3
4
5
t [s]
v [
m/s
]
UGV x velocity
UGV y velocity
Velocity bounds
0 2 4 6 8 10 12 14 16 18
-4
-2
0
2
4
6
8
t [s]
v [
m/s
]
UAV x velocity
UAV y velocity
Velocity bounds
-
33
as possible to the fire. A similar behavior is observed in UAV velocity when the UAV gets close
to the UGV and the fire.
2.6 Summary
This chapter presented a motion planning and control framework using nonlinear model
predictive control. The objective of the proposed system was to drive the unmanned vehicles in
the environment, safely and optimally. To deal with uncertainties, different state estimation
algorithms (including filtering and prediction algorithms) were studied and developed for linear
and nonlinear systems. The presented motion planning framework was used in an automated
firefighting application. More deeply, the cooperative setting consisted of one UAV and one
UGV that cooperated to handle the fire in a firefighting task. The proposed automated system
aimed at driving the unmanned vehicles from their initial states to the targeted states in an
optimal way. The motion planning scheme intended to find a time-fuel optimal trajectory for the
unmanned vehicles while maximizing the state estimation accuracy and handling constraints.
The vehicles motion models and measurement functions were assumed to be linear. The partially
closed-loop state estimation algorithm was adopted to predict an estimation of the future states.
Various constraints including obstacle avoidance for the UGV, no-fly zone avoidance for the
UAV, and bounded control inputs were considered to study the ability of the proposed
framework to provide a practical solution for real-world scenarios. In order to verify the
applicability of the proposed scheme, numerical simulations were carried out. Based on the
defined scenario and the problem formulation, two major behaviors were expected to be
observed in the simulation results as follows.
A safe optimal path is designed for the UGV to reach to the fire.
-
34
The UAV path is designed to maximize the estimation accuracy and to avoid the no-fly
zones while being robust against noise and disturbances.
Simulation results verified our initial perception of the system behavior. In the presence of
obstacles, the UGV chooses the shortest possible path to reach the target that is robust and avoids
the obstacle. Furthermore, the UAV decreases its distance with the fire and the UGV in order to
minimize the system data uncertainty. It is notable that in the RHC, the optimal control problem
is solved over a finite horizon. Thus, the solution is sub-optimal unless the final cost function
represents an exact cost-to-go from the last state in the horizon. As a result, globally optimal
solutions are not guaranteed to be obtained. We would like to clarify that the so-called optimal
solution is optimal in the sense that it is a result of an optimization problem. However, it is in
fact sub-optimal as the planning horizon is finite and the final cost function is never guaranteed
to be exact.
In summary, simulation results demonstrated that the developed RHC motion planning
scheme is able to design trajectories for both the UGV and the UAV that are optimal (within the
problem constraints and in the sense that they are resulted from an optimization framework), safe
(in the sense of collision avoidance) and satisfy the design criteria i.e., the system uncertainty is
minimized. It should also be mentioned that the developed algorithm is able to solve the motion
planning problem given that an optimal feasible solution for the problem exists. Obviously, in
cases where there is no feasible solution to the problem that can satisfy all of the constraints, the
proposed scheme is not able to find a solution.
-
35
Chapter 3
MILP Receding Horizon Trajectory Planning
3.1 Background and Overview
This chapter presents a receding horizon motion planning scheme for trajectory planning of
robots using mixed integer linear programming (MILP). In Chapter 2, RHC-based motion
planning was developed for a general case assuming nonlinear system and measurement
equations. Under certain assumptions i.e., linear system equations and convex polyhedral
obstacles, the motion planning problem can be formulated as an RHMILP [56]. Schouwenaars et
al. first introduced RHMILP as a tool for motion planning in the presence of convex polyhedral
obstacles and absence of uncertainties in the environment [56]. In 2006, a distributed RHMILP
framework for multivehicle path planning was presented in [71] that aimed to maintain LOS
between the vehicles during operation. In 2011, Kuwata and How developed a robust RHMILP
framework that utilized set theory to characterize uncertainties [49]. In 2013, Richards and
Turnbull developed a new MILP-based obstacle avoidance constraint that