Motion planning based on uncertain robot states in dynamic environments : a receding horizon control...

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.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.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



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)


{

| ( |

)

| | ( ( | ))

| ( ) |

(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)


{

| ( |

)

| |

| ( ) |

(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

Motion planning based on uncertain robot states in dynamic environments : a receding horizon control...

Documents

Transcript of Motion planning based on uncertain robot states in dynamic environments : a receding horizon control...