The Generalized Label Correcting Method for Optimal Kinodynamic Motion Planning

TL;DR

This thesis introduces a generalized label correcting algorithm for optimal kinodynamic motion planning that does not require local planning subroutines, improving efficiency and applicability across diverse robotic applications.

Contribution

It develops a resolution complete, optimal kinodynamic motion planning algorithm based on direct forward search and introduces a method for deriving admissible heuristics using convex optimization.

Findings

The algorithm effectively prunes suboptimal input signals, reducing computational effort.

A new admissibility condition for heuristics is established, ensuring optimality.

A sum-of-squares relaxation provides provably admissible approximate solutions.

Abstract

Nearly all autonomous robotic systems use some form of motion planning to compute reference motions through their environment. An increasing use of autonomous robots in a broad range of applications creates a need for efficient, general purpose motion planning algorithms that are applicable in any of these new application domains. This thesis presents a resolution complete optimal kinodynamic motion planning algorithm based on a direct forward search of the set of admissible input signals to a dynamical model. The advantage of this generalized label correcting method is that it does not require a local planning subroutine as in the case of related methods. Preliminary material focuses on new topological properties of the canonical problem formulation that are used to show continuity of the performance objective. These observations are used to derive a generalization of Bellman's principle of optimality in the context of kinodynamic motion planning. A generalized label correcting algorithm is then proposed which leverages these results to prune candidate input signals from the search when their cost is greater than related signals. The second part of this thesis addresses admissible heuristics for kinodynamic motion planning. An admissibility condition is derived that can be used to verify the admissibility of candidate heuristics for a particular problem. This condition also characterizes a convex set of admissible heuristics. A linear program is formulated to obtain a heuristic which is as close to the optimal cost-to-go as possible while remaining admissible. This optimization is justified by showing its solution coincides with the solution to the Hamilton-Jacobi-Bellman equation. Lastly, a sum-of-squares relaxation of this infinite-dimensional linear program is proposed for obtaining provably admissible approximate solutions.

Figures

Tables

Table 1. Table 5.1: Tuning parameter selection for the shortest path example.

Resolution range: $R$	$R=\{20,25,\dots ,200\}$
Horizon limit: $h\left(R\right)$	$100R\log \left(R\right)$
Partition scaling: $\eta \left(R\right)$	$R^2/300$
Control primitive duration: $\gamma /R$	$10/R$

Table 2. Table 5.2: Tuning parameter selection for the pendulum swing-up example.

Resolution range: $R$	$R=\{4,5,6,7,8\}$
Horizon limit: $h\left(R\right)$	$100R\log \left(R\right)$
Partition scaling: $\eta \left(R\right)$	$R^{5/2}/16$
Control primitive duration: $\gamma /R$	$6/R$

Table 3. Table 5.3: Tuning parameter selection for the acrobot swing-up example.

Resolution range:	$R\in \{4,5,\dots ,10\}$
Horizon limit: $h\left(R\right)$	$100R\log \left(R\right)$
Partition scaling: $\eta \left(R\right)$	$R^2/16$
Control primitive duration: $\gamma /R$	$6/R$

Table 4. Table 5.4: Tuning parameter selection for the thrust limited 3D point robot example.

Resolution range:	$R\in \{8,9,\dots ,12\}$
Horizon limit: $h\left(R\right)$	$100R\log \left(R\right)$
Partition scaling: $\eta \left(R\right)$	$R^2/64$
Control primitive duration: $\gamma /R$	$10/R$

Table 5. Table 5.5: Tuning parameter selection for the wheeled robot example.

Resolution range:	$R\in \{4,5,\dots ,9\}$
Horizon limit: $h\left(R\right)$	$5R\log \left(R\right)$
Partition scaling: $\eta \left(R\right)$	$R^{5/\pi }/15$
Control primitive duration: $\gamma /R$	$10/R$

Equations

$$M(q(t),\dot{q}(t))\ddot{q}(t)+C(q(t),\dot{q}(t))\dot{q}(t)+G(q(t))=B(q(t))\tau (t),$$

$$\mathcal{U}:=\bigcup _{\tau >0}\left\{u\in L_1([0,\tau ]):u(t)\in \Omega \forall t\in [0,\tau ]\right\},$$

$${\mathcal{X}}_{x_0}:=\bigcup _{\tau >0}\left\{x:[0,\tau ]\to {\mathbb{R}}^n:x(0)=x_0,\mathrm{Lip}(x)\le M\right\},$$

$$x(t)=x_0+{\int }_{[0,t]}f(x(t),u(t))\mu (dt).$$

$${\mathcal{X}}_{\mathrm{free}}:=\left\{x\in {\mathcal{X}}_{x_{\mathrm{ic}}}:x(t)\in X_{\mathrm{free}}\forall t\in [0,\tau (x)]\right\}.$$

$${\mathcal{X}}_{\mathrm{goal}}:=\left\{x\in {\mathcal{X}}_{\mathrm{free}}:x(\tau (x))\in X_{\mathrm{goal}}\right\}.$$

$${\mathcal{U}}_{\mathrm{free}}={\phi }_{x_{ic}}^{-1}({\mathcal{X}}_{\mathrm{free}}),{\mathcal{U}}_{\mathrm{goal}}={\phi }_{x_{ic}}^{-1}({\mathcal{X}}_{\mathrm{goal}}).$$

$$J_{x_0}(u)={\int }_{[0,\tau (u)]}g([{\phi }_{x_0}(u)](t),u(t))\mu (dt),$$

$$\underset{R\to \infty }{\lim }{J}_{x_{\mathrm{ic}}}(u_R)=\underset{u\in {\mathcal{U}}_{\mathrm{goal}}}{\inf }{J}_{x_{\mathrm{ic}}}(u):=c^{\ast }.$$

$$B_{\delta }(x(t))\subset X_{\mathrm{free}},\forall t\in [0,\tau (x_1)].$$

$$d_{\mathcal{U}}(u_1,u_2):={\int }_{[0,\min \{\tau (u_1),\tau (u_2)\}]}\Vert u_1(t)-u_2(t){\Vert }_2\mu (dt)+u_{max}|\tau (u_1)-\tau (u_2)|.$$

$$d_{\mathcal{X}}(x_1,x_2):=\underset{t\in \left[0,\min \{\tau (x_1),\tau (x_2)\}\right]}{\max }\left\{\Vert x_1(t)-x_2(t)\Vert \right\}+M|\tau (x_1)-\tau (x_2)|.$$

$${\Vert x_1(t)-x_2(t)\Vert }_2\le \Vert x_0-z_0{\Vert }_2{e}^{L_{f}t}.$$

$$\begin{array}[]{rcl}\|x_{1}(t)-x_{2}(t)\|_{2}&=&\left\|\int_{[0,t]}f(x_{1}(\tau),u_{1}(\tau))\,\mu(d\tau)-\int_{[0,t]}f(x_{2}(\tau),u_{2}(\tau))\,\mu(d\tau)\right\|_{2}\\ &\leq&\int_{[0,t]}\left\|f(x_{1}(\tau),u_{1}(\tau))-f(x_{2}(\tau),u_{2}(\tau))\,\right\|_{2}\mu(d\tau).\end{array}$$

$$\begin{array}[]{rcl}\|x_{1}(t)-x_{2}(t)\|_{2}&\leq&\int_{[0,t]}L_{f}\|x_{1}(\tau)-x_{2}(\tau)\|_{2}+L_{f}\|u_{1}(\tau)-u_{2}(\tau)\|_{2}\,\mu(d\tau)\\ \|x_{1}(t)-x_{2}(t)\|_{2}&\leq&\int_{[0,t]}L_{f}\|x_{1}(\tau)-x_{2}(\tau)\|_{2}\,\mu(d\tau)+L_{f}\int_{[0,t]}\|u_{1}(\tau)-u_{2}(\tau)\|_{2}\,\mu(d\tau)\\ &\leq&L_{f}d_{\mathcal{U}}(u_{1},u_{2})+\int_{[0,t]}L_{f}\|x_{1}(\tau)-x_{2}(\tau)\|_{2}\,\mu(d\tau).\end{array}$$

$$\Vert x_1(t)-x_2(t){\Vert }_2\le L_f{d}_{\mathcal{U}}(u_1,u_2)e^{L_{f}t}.$$

$$\begin{array}[]{rcl}M|\tau(x_{1})-\tau(x_{2})|&=&M|\tau(u_{1})-\tau(u_{2})|\\ &<&Md_{\mathcal{U}}(u_{1},u_{2})/u_{max}.\end{array}$$

$$\begin{array}[]{rcl}d_{\mathcal{X}}(x_{1},x_{2})&=&\underset{{t\in[0,\min\{\tau(x_{1}),\tau(x_{2})\}]}}{\max}\{\|x_{1}(t)-x_{2}(t)\|_{2}\}+M|\tau(x_{1})-\tau(x_{2})|\\ &<&L_{f}d_{\mathcal{U}}(u_{1},u_{2})e^{L_{f}\tau(u_{1})}+Md_{\mathcal{U}}(u_{1},u_{2})/u_{max}\\ &=&(L_{f}e^{L_{f}\tau(u_{1})}+M/u_{max})d_{\mathcal{U}}(u_{1},u_{2}).\end{array}$$

$$\underset{t\in \left[0,\min \{\tau (x_1),\tau (x_2)\}\right]}{\max }\left\{{\Vert x_1(t)-x_2(t)\Vert }_2\right\}+M|\tau (x_1)-\tau (x_2)|<\rho /2.$$

$$\Vert x_2(t)-x_1(\tau (x_1)){\Vert }_2\le \Vert x_1(\tau (x_1))-x_2(\tau (x_1)){\Vert }_2+\Vert x_2(\tau (x_1))-x_2(t){\Vert }_2.$$

$$\begin{array}[]{rcl}\|x_{2}(t)-x_{1}(\tau(x_{1}))\|_{2}&\leq&\|x_{1}(\tau(x_{1}))-x_{2}(\tau(x_{1}))\|_{2}+M|t-\tau(x_{1})|\\ &\leq&\|x_{1}(\tau(x_{1}))-x_{2}(\tau(x_{1}))\|_{2}+M|\tau(x_{2})-\tau(x_{1})|\\ &\leq&\rho.\end{array}$$

$$\underset{t\in \left[0,\min \{\tau (x_1),\tau (x_2)\}\right]}{\max }\left\{{\Vert x_1(t)-x_2(t)\Vert }_2\right\}+M|\tau (x_1)-\tau (x_2)|<\rho /2.$$

$$|\tau (x_1)-\tau (x_2)|<\rho /(2M).$$

$$\Vert x_1(\min \{\tau (x_1),\tau (x_2)\})-x_1(\min \{\tau (x_1),\tau (x_2)\}){\Vert }_2<\rho /2.$$

$$\begin{array}[]{rcl}\|x_{2}(\tau(x_{2}))-x_{1}(\tau(x_{1}))\|_{2}&\leq&\|x_{2}(\tau(x_{2}))-x_{1}(\tau(x_{2}))\|_{2}+\|x_{1}(\tau(x_{1}))-x_{1}(\tau(x_{2}))\|_{2}\\ &\leq&\rho/2+M|\tau(x_{2})-\tau(x_{1})|\\ &\leq&\rho/2+\frac{M\rho}{2M}\\ &\leq&\rho.\end{array}$$

$$\begin{array}[]{rcl}\|x_{2}(\tau(x_{2}))-x_{1}(\tau(x_{1}))\|_{2}&\leq&\|x_{2}(\tau(x_{1}))-x_{1}(\tau(x_{1}))\|_{2}+\|x_{2}(\tau(x_{1}))-x_{2}(\tau(x_{2}))\|_{2}\\ &\leq&\rho/2+M|\tau(x_{2})-\tau(x_{1})|\\ &\leq&\rho/2+\frac{M\rho}{2M}\\ &\leq&\rho.\end{array}$$

$${\mathcal{U}}_{\mathrm{free}}={\phi }_{x_{ic}}^{-1}({\mathcal{X}}_{\mathrm{free}}),{\mathcal{U}}_{\mathrm{goal}}={\phi }_{x_{ic}}^{-1}({\mathcal{X}}_{\mathrm{goal}}).$$

$$\begin{array}[]{rcl}\left|J_{x_{0}}(u_{1})-J_{x_{0}}(u_{2})\right|&=&\left|\int_{\left[0,\tau(u_{1})\right]}g\left(x_{1}(t),u_{1}(t)\right)\,\mu(dt)-\int_{\left[0,\tau(u_{2})\right]}g\left(x_{2}(t),u_{2}(t)\right)\,\mu(dt)\right|\\ &\leq&\left|\int_{\left[0,\tau(u_{1})\right]}g\left(x_{1}(t),u_{1}(t)\right)\right.-g\left(x_{2}(t),u_{2}(t)\right)\,d\mu(t)\\ &&-\left.\int_{\left[\tau(u_{1}),\tau(u_{2})\right]}g\left(x_{2}(t),u_{2}(t)\right)\,d\mu(t)\right|.\\ &\leq&\int_{\left[0,\tau(u_{1})\right]}\left|g\left(x_{1}(t),u_{1}(t)\right)-g\left(x_{2}(t),u_{2}(t)\right)\,\right|\mu(dt)\\ &&\left.+\int_{\left[\tau(u_{1}),\tau(u_{2})\right]}\left|g\left(x_{2}(t),u_{2}(t)\right)\,\right|\mu(dt).\right.\end{array}$$

$$\begin{array}[]{rcl}\left|J_{x_{0}}(u_{1})-J_{x_{0}}(u_{2})\right|&\leq&\int_{\left[0,\tau(u_{1})\right]}L_{g}\left\|x_{1}(t)-x_{2}(t)\right\|_{2}+L_{g}\left\|u_{1}(t)-u_{2}(t)\right\|_{2}\,\mu(dt)\\ &&+\int_{\left[\tau(u_{1}),\tau(u_{2})\right]}g\left(x_{2}(t),u_{2}(t)\right)\,\mu(dt)\\ &\leq&L_{g}\tau(u_{1})d_{\mathcal{X}}(x_{1},x_{2})+L_{g}d_{\mathcal{U}}(u_{1},u_{2})\\ &&+\int_{\left[\tau(u_{1}),\tau(u_{2})\right]}g\left(x_{2}(t),u_{2}(t)\right)\,\mu(dt).\end{array}$$

$$\begin{array}[]{rcl}\left|J_{x_{0}}(u_{1})-J_{x_{0}}(u_{2})\right|&\leq&L_{g}\tau(u_{1})d_{\mathcal{X}}(x_{1},x_{2})+L_{g}d_{\mathcal{U}}(u_{1},u_{2})+G|\tau(u_{2})-\tau(u_{1})|.\end{array}$$

Full text

\department

Department of Mechanical Engineering

\degree

Doctor of Philosophy

\degreemonth

June \degreeyear2017 \thesisdateJune 5, 2017

\supervisor

Emilio FrazzoliProfessor of Aeronautics and Astronautics \committeeDomitilla Del VecchioAssociate Professor of Mechanical EngineeringChair, Thesis Committee \committeeKarl IagnemmaPrincipal Research ScientistMember, Thesis Committee

\chairman

Rohan AbeyaratneChair, Department Committee on Graduate Theses

A Generalized Label Correcting Method for Optimal Kinodynamic Motion Planning

Brian A. Paden

{abstractpage}

Nearly all autonomous robotic systems use some form of motion planning to compute reference motions through their environment. An increasing use of autonomous robots in a broad range of applications creates a need for efficient, general purpose motion planning algorithms that are applicable in any of these new application domains.

This thesis presents a resolution complete optimal kinodynamic motion planning algorithm based on a direct forward search of the set of admissible input signals to a dynamical model. The advantage of this generalized label correcting method is that it does not require a local planning subroutine as in the case of related methods.

Preliminary material focuses on new topological properties of the canonical problem formulation that are used to show continuity of the performance objective. These observations are used to derive a generalization of Bellman’s principle of optimality in the context of kinodynamic motion planning. A generalized label correcting algorithm is then proposed which leverages these results to prune candidate input signals from the search when their cost is greater than related signals.

The second part of this thesis addresses admissible heuristics for kinodynamic motion planning. An admissibility condition is derived that can be used to verify the admissibility of candidate heuristics for a particular problem. This condition also characterizes a convex set of admissible heuristics. A linear program is formulated to obtain a heuristic which is as close to the optimal cost-to-go as possible while remaining admissible. This optimization is justified by showing its solution coincides with the solution to the Hamilton-Jacobi-Bellman equation. Lastly, a sum-of-squares relaxation of this infinite-dimensional linear program is proposed for obtaining provably admissible approximate solutions.

Acknowledgments

I would like to express my gratitude to my advisor Professor Emilio Frazzoli for his input and support through my graduate studies. Emilio has given me tremendous freedom in pursuing the research questions I found most interesting and provided skillful guidance to keep me moving in the right direction. I have also enjoyed numerous opportunities that were a direct consequence of my affiliation with Emilio’s research group. Among these were several exciting internships working on autonomous systems and the opportunity to spend a year doing research at ETH.

I am also grateful to my committee members, Professor Domitilla Del Vecchio and Karl Iagnemma for their valuable feedback and advice in out meetings over the past several years.

Additionally, I would like to thank my colleagues and friends with whom I have worked over the past four years. Sze Zheng Yong was a great mentor in my first year at MIT and we were able to collaborate on a number of interesting projects. The afternoon chalkboard sessions with Dmitry Yershov were stimulating and memorable. They contributed significantly to the ideas in the first part of this thesis. The second part of this thesis related to heuristics for kinodynamic planning problems came from a collaboration with Valerio Varricchio on a course project in Professor Pablo Parrilo’s algebraic techniques class.

For Stephanie

Contents

1. 1 Introduction

2. 1.1 Background and Motivation

3. 1.1.1 Methods from Optimal Control

4. 1.1.2 Methods from Computational Geometry

5. 1.1.3 Approximate Methods for Motion Planning

6. 1.2 Statement of Contributions

7. 2 Problem Formulation

8. 2.1 Decision Variables

9. 2.2 Problem Specifications

10. 2.3 Review of Assumptions

11. 2.3.1 Verifiability of Assumptions

12. 3 Topological Properties of the Input Signal and Trajectory Spaces

13. 3.1 Metrics on the Signal and Trajectory Spaces

14. 3.2 Continuity of the System Map $\varphi_{x_{0}}$ and Cost Functional $J_{x_{0}}$

15. 3.3 Properties of the Set of Solutions

16. 4 Approximation of the Space of Input Signals

17. 4.1 Consistency of the Approximation

18. 5 The Generalized Label Correcting Method

19. 5.1 Constructing Equivalence Classes of Signals

20. 5.2 Properties of the $\mathrm{GLC}$ Conditions

21. 5.3 A $\mathrm{GLC}$ Algorithm

22. 5.4 Proof of Resolution Completeness

23. 5.5 Kinodynamic Motion Planning Examples

24. 5.5.1 Shortest Path Problem

25. 5.5.2 Torque-Limited Pendulum Swing-Up

26. 5.5.3 Torque Limited Acrobot Swing-Up

27. 5.5.4 Thrust Limited 3D Point Robot

28. 5.5.5 Nonholonomic Wheeled Robot

29. 5.5.6 Observations and Discussion

30. 6 Admissible Heuristics for Optimal Kinodynamic Motion Planning

31. 6.1 Graph-Based Approximations

32. 6.1.1 Admissible Heuristics

33. 6.2 Selecting an Admissible Heuristic

34. 6.2.1 Problem Relaxations

35. 6.3 Sum-of-Squares (SOS) Approximation to Equation (LP)

36. 6.3.1 Sum-of-Squares Polynomials

37. 6.3.2 Optimizing the Heuristic

38. 6.4 Examples of Heuristics for Kinodynamic Motion Planning

39. 6.4.1 Admissibility of Euclidean Distance in Shortest Path Problems

40. 6.4.2 Wheeled Robot Example

41. 6.4.3 Autonomous Underwater Vehicle Example

42. 6.5 Sum-of-Square Heuristic Synthesis Examples

43. 6.5.1 Single Integrator Example

44. 6.5.2 Double Integrator Example

45. 6.5.3 Observations

46. 7 Conclusion

47. 7.1 Extensions and Improvements

48. A Review of Set Theory

49. A.1 Notation

50. A.2 Operations on Sets

51. A.3 Binary Relations

52. A.3.1 Functions

53. A.3.2 Orders

54. A.3.3 Equivalence Relations

55. B Review of Mathematical Analysis

56. B.1 Topological Spaces

57. B.1.1 Compactness

58. B.1.2 Continuity

59. B.2 Metric Spaces

60. B.2.1 Continuity in Metric Spaces

61. B.2.2 Complete Metric Spaces

62. B.3 Normed Vector Spaces

63. B.3.1 Banach Spaces

64. C Analysis of Functions and Integration

65. C.1 Measure Spaces

66. C.1.1 Lebesgue Integration

67. C.2 Integral Equations (Dynamical System Models)

68. D Graph Search Algorithms

69. E Extended Derivations and Proofs

70. E.1 Proof that $d_{\mathcal{U}}$ is a Metric on $\mathcal{U}$

71. E.2 Proof that ${d_{\mathcal{X}}}$ is a metric on $\mathcal{X}$

72. E.3 Derivation of Inequality (5.4.2)

Chapter 1 Introduction

Robotics and automation have been reshaping the world economy with widespread use in industries such as manufacturing, agriculture, transportation, defense, and medicine. Advances to the theoretical foundations of the subject together with a seemingly endless stream of new sensing and computing technology is making autonomous robots capable of increasingly complex tasks. One of the emerging new applications is driverless vehicles [padenSurvey] and transportation systems which have the potential to eliminate urban congestion [spieser2014toward] and dramatically reduce the roughly 1.24 million lives lost each year in road traffic accidents [world2013global].

The sophistication and complexity of modern robotic systems requires entire fields of research devoted to individual subsystems. These can be divided into two major categories. The first, sensing and perception which inform the system about its state and the surrounding environment; the second, planning and control which makes use of sensor data, processed by the perception system, and selects appropriate actions to accomplish task specifications.

While both of these are subjects of active research, the scope of this thesis falls into the latter category of planning and control. Specifically, it focuses on the planning of motions through an environment that satisfy a dynamical model of the system while optimizing a measure of performance.

1.1 Background and Motivation

Research on planning and control for robotic systems is addressed with a combination of techniques from theoretical computer science and control theory. High level decision making and task planning problems generally involve making selections from a discrete set of alternatives. The techniques from theoretical computer science lend themselves well to these problems given their discrete nature. Central research questions involve: determining expressive problem formulations that can be utilized in a large variety of applications, determining the computational complexity of a particular problem formulation, and then designing sound and correct algorithms which realize this complexity. At the other end of the spectrum, a feedback control system faithfully executes reference motions given a potentially complex dynamical model of the system. Central research questions in control theory are the stability of the system in the presence of feedback control, and robustness to disturbances and modeling errors.

While computer science provides the tools for high level planning and control theory provides the tools to execute a reference motion, the actual planning of a robot’s reference motion falls in the middle ground between these two disciplines and has been treated almost independently by the two. The main specifications for the planned motion are: (i) the motion must originate from a prespecified initial state and terminate in a set of goal states; (ii) it must satisfy constraints on the state, such as obstacle avoidance, along the planned motion; (iii) there must exist a control signal which, together with the planned motion, satisfies the differential equation modeling the system; (iv) lastly, a measure of cost must be minimized.

1.1.1 Methods from Optimal Control

A number of techniques for solving this problem have been developed within the optimal control literature where greater emphasis has been placed on dealing with differential constraints and less on state constraints. The classical approach is to use extensions of the calculus of variations to express necessary first order optimality conditions of a solution. One celebrated approach is known as Pontryagin’s minimum principle after Lev Pontryagin who pioneered the extension to optimal control [pontryagin]. The first order optimality conditions are a number of differential equations that must satisfy initial and terminal state constraints, also known as a two-point boundary value problem. The two-point boundary value problem can be solved in closed form in a number of examples, but in general it requires a numerical method to determine the appropriate boundary values. The shooting method is the standard numerical method for solving the two-point boundary value problem. However, extreme sensitivity of the terminal boundary value with respect to the initial boundary value makes it difficult to solve in most examples [brysonapplied, pg. 214].

Direct methods have become a favored approach. These methods approximate the trajectory and control as a finite-dimensional vector space and then directly optimize the performance objective with a nonlinear programming algorithm. Some of the popular approaches are the direct shooting method, reviewed in [betts1998survey], which is particularly easy to implement; orthogonal collocation methods [benson2006direct] which offer a good numerical approximation with a relatively small number of basis vectors; and direct collocation methods [hargraves1987direct] which provide an approximation resulting in a sparse Hessian matrix for the performance objective.

The direct optimization methods rapidly converge to a locally optimal solution when they are given a suitable initial guess. However, constructing an initial guess is more of an art than a science and if the initial guess does not satisfy the constraints, many solvers may fail to find a feasible solution at all. Additionally, direct optimization methods will converge to a locally optimal solution which may be unacceptably different than any globally optimal solutions. These issues are exacerbated in complex environments that can introduce numerous local minima in the performance objective. Thus, direct optimization methods are not sound in the sense that they may fail to find a solution when one exists. In real-time motion planning applications where the output of the motion planner can be safety critical, these methods need to be used with a great deal of caution.

1.1.2 Methods from Computational Geometry

A classic problem in computational geometry is the mover’s problem (also called the piano or couch mover’s problem) which had received attention from the computational geometry community during the 1970s. A generalization of the mover’s problem was gaining interest with the increasing use of industrial manipulators in manufacturing in the late 1970s. The generalized mover’s problem consider’s planning a motion for multiple polyhedra freely linked at distinguished vertices (to model an industrial manipulator arm).

In a famous paper by Reif [reif1979complexity], an algorithm is provided for the mover’s problem whose complexity is polynomial in the number of constraints defined by the obstacles. Further, Reif shows that the generalized mover’s problem is PSPACE-hard with respect to the number of degrees of freedom meaning that the difficulty of solving motion planning problems grows rapidly as the degrees of freedom increase. Several years later Schwartz and Sharir presented a cell-decomposition algorithm [schwartz1983piano] which solved the generalized mover’s problem for algebraic (instead of polyhedral) bodies with complexity in $O((2n)^{3^{r+1}}\cdot m^{2^{r}})$ where $r$ is proportional to the number of degrees of freedom of the robot, $n$ is proportional to the algebraic degree of the constraints, and $m$ is the number of polynomials describing the constraints. The cell decomposition algorithm is most often cited for the doubly exponential complexity in the degrees of freedom of the robot which is not so disappointing considering Reif’s results several years earlier. However, the important observation is that the complexity is polynomial with respect to the number of obstacles in the environment. Canny later provided an algorithm that solved the generalized mover’s problem with complexity which is only exponential in the degrees of freedom [canny1987new].

In contrast to the trajectory optimization problems addressed by the optimal control community, the classical kinematic motion planning problems are only concerned with obstacle avoidance. For robot manipulators there is a straight-forward justification for neglecting the robot dynamics. The dynamics of robot manipulators determined by the principles of classical mechanics [abraham1978foundations] can almost universally be written as

[TABLE]

where $q(t)$ is the vector of generalized coordinates for the robot’s configuration and $\tau(t)$ is the vector of generalized control forces applied by the actuators. When there is at least one actuator for each generalized coordinate and the matrix $B$ has rank equal to the number of generalized coordinates for every configuration, any twice-differentiable time parameterization of a planned motion $q$ can be executed with the control forces $\tau(t)$ solving the manipulator equation (1.1.1). When these conditions are met, the robot is said to be fully actuated which is typically the case for robot manipulators.

1.1.3 Approximate Methods for Motion Planning

The growing robotics industry in the late 1980s and 1990s called for practical solutions to the motion planning problem. With the disappointing complexity of available complete algorithms, researchers began developing practical techniques without theoretical guarantees, that worked well in practice [khatib1986real, hwang1992potential, rimon1992exact].

Another practical approach with some theoretical justification was to seek methods with resolution completeness [brooks1985subdivision], meaning that a the output of the algorithm is correct for some (not known a-priori) sufficiently high resolution. The analogue for randomized algorithms is probabilistic completeness [barraquand1996random]. The concept of probabilistic completeness was introduced at around the same time as the probabilistic roadmap ($\mathrm{PRM}$) algorithm [kavraki1996probabilistic], whose effectiveness triggered a paradigm shift in motion planning research towards sampling-based approximate methods. One of the more attractive features of the algorithm is that the probability of failing to correctly determine the feasibility of a motion planning problem converges to [math] at an exponential rate in the number of random samples. The basic principle of the $\mathrm{PRM}$ algorithm is to randomly sample a large number admissible robot configurations, and then construct a graph by connecting nearby configurations with an edge if the line between them does not contain inadmissible configurations.

With the $\mathrm{PRM}$ algorithm effectively solving classical motion planning problems, attention turned to planning for systems where dynamical constraints cannot be neglected or kinodynamic motion planning. The difficulty in applying the $\mathrm{PRM}$ to kinodynamic motion planning is that the construction of edges by linear interpolation between configurations may not be a dynamically feasible motion. A simple adaptation is to replace linear interpolation by a local planning or steering subroutine, but this complicates individual implementations and places a burden on the user of the algorithm to provide this subroutine which itself must solve a kinodynamic motion planning problem. The first algorithms addressing the kinodynamic motion planning problem without a steering subroutine were the expansive space trees ($\mathrm{EST}$) algorithm [EST_Journal] and the rapidly exploring random tree ($\mathrm{RRT}$) algorithm [RRT_Journal]; both of which relied on forward integration of the dynamics with random control inputs instead of a point-to-point local planning subroutine. Further, both methods boasted probabilistic completeness like the $\mathrm{PRM}$ algorithm.

The next major development in sampling-based motion planning was optimal variations of the $\mathrm{PRM}$ and $\mathrm{RRT}$, denoted $\mathrm{PRM}^{*}$ and $\mathrm{RRT}^{*}$ [karaman2011sampling] developed by Karaman and Frazzoli. The innovation was in the selection of edges in the $\mathrm{PRM}$ graph. Karaman and Frazzoli described how to construct the graph to be as sparse as possible while ensuring asymptotic optimality with respect to a performance objective in addition to probabilistic completeness. The $\mathrm{RRT}^{*}$ algorithm is in fact more closely related to the $\mathrm{PRM}$ than the $\mathrm{RRT}$. The $\mathrm{RRT}^{*}$ algorithm is essentially an incremental version of the $\mathrm{PRM}^{*}$ algorithm which simultaneously constructs a minimum spanning tree in the graph from an initial configuration.

While the $\mathrm{RRT}^{*}$ and $\mathrm{PRM}^{*}$ algorithms were highly impactful, a local planning subroutine was once again required. Nonetheless, the utility of the $\mathrm{PRM}^{*}$ and $\mathrm{RRT}^{*}$ stimulated significant research efforts towards finding general methods for solving the local steering problem. For systems with linear dynamics, classical solutions from linear systems theory can be applied to the local planning subroutine. The drawback to this approach is that the time spent on local planning is prohibitive, taking several minutes for $\mathrm{RRT}^{*}$ to produce satisfactory solutions in the examples presented in [webb2013kinodynamic] and [perez2012lqr]. Additionally, many systems of interest are non-linear making this approach limited in scope.

More recently, the $\mathrm{SST}$ algorithm [li2015sparse] was proposed which provided an algorithm converging asymptotically to an approximately optimal solution without the use of a local planning subroutine. This offers an advantage over $\mathrm{RRT}^{*}$ in problems where the local planning subroutine is not available in closed form. However, $\mathrm{SST}$ still requires significant running time making it difficult to apply to real-time planning.

1.2 Statement of Contributions

This thesis presents a number of theoretical contributions related to optimal kinodynamic motion planning. The principal contribution is a resolution complete optimal kinodynamic motion planning algorithm based on a direct forward search of the set of input control signals.

Chapters 2 and 3 review the problem formulation addressed, as well as a careful investigation into topological properties of the set of solutions. The key observation in these chapters is continuity of the performance objective with respect to the input signal, presented in Lemma 3.3.3, and a bound on the sensitivity of the cost function with respect to initial conditions, presented in Lemma 3.3.4.

The results of Chapter 3 are the basis of a generalization of classical label correcting algorithms where the comparison of relative cost can be made, not only between trajectories terminating at the same state, but between all trajectories terminating in a particular region of the state space. The generalized label correcting conditions, described in Chapter 5, specify which segments of trajectories can be discarded from the search without compromising convergence to the optimal solution with increasing resolution. The generalized label correcting conditions presented in this thesis are a sharper version of the conditions presented in [paden2016generalized] resulting in faster algorithm run-times. Theorem 5.2.1 is a restatement of Bellman’s principle of optimality in the context of kinodynamic motion planning and taking into account the topological properties established in Chapter 3. From this result, resolution completeness of the generalized label correcting method follows in Corollary 5.4.1 in the same way that completeness of a label correcting algorithm follows from the principle of optimality in graph search problems.

A wide range of numerical experiments are presented in Chapter 5 which confirm the theoretical results as well as suggest that this method is suitable for real-time planning applications. To further improve the running time of the algorithm, Chapter 6 addresses admissible heuristics for kinodynamic motion planning problems. An admissibility condition for candidate heuristics is presented in Theorem 6.1.1 that provides a tool for verifying the admissibility of candidate heuristics. Further, this condition characterizes a convex set of admissible heuristics which contains the optimal cost-to-go for a particular problem. An infinite-dimensional linear program is formulated to optimize over the set of admissible heuristics, and it is shown in Theorem 6.2.1 that this linear program is equivalent to the Hamilton-Jacobi-Bellman equation. Lastly, a relaxation of this linear program as a sum-of-squares program is proposed which provides provably admissible heuristics which are as close as possible to the optimal cost-to-go within a finite-dimensional subspace of polynomials.

To create a self-contained thesis, appendices containing a concise review of mathematical analysis, dynamical systems theory, and graph search algorithms are provided at the end of this document.

Chapter 2 Problem Formulation

Consider a controlled dynamical system whose state space is $\mathbb{R}^{n}$, and whose input space is $\mathbb{R}^{m}$. Canonical state variables for a robot include generalized coordinates and momenta, but can also include relevant quantities such as currents and voltages in electronics. Similarly, canonical input variables for a robot are the generalized forces applied to the robot, but higher fidelity models might include the torque as a state responding to inputs from a drive-train.

To reflect constraints such as obstacle avoidance and velocity limits, the state is restricted to remain in an open111with respect to the standard topology on $\mathbb{R}^{n}$. subset of admissible states $X_{\mathrm{free}}$ of $\mathbb{R}^{n}$ at each instant in time. Similarly, to reflect the design limitations of the actuators, the control is restricted to a bounded subset of admissible control inputs $\Omega$ of $\mathbb{R}^{m}$ at each instant in time. Additionally, the motion planning objective is encoded with a terminal constraint that the executed motion terminates in an open set of goal states $X_{\mathrm{goal}}$ in $\mathbb{R}^{n}$.

A continuous function $x$ from a closed interval into $\mathbb{R}^{n}$ is called a trajectory. A trajectory is a time history of states over some time interval. If $x$ is a trajectory, then $x(t)$ is the state on that trajectory at time $t$. A measurable function $u$ from a closed time interval into $\Omega$ is called an input signal. Like a trajectory, an input signal is a time history, now of control inputs. Instantaneous changes in input are permitted as long as the control signal is mathematically well behaved (measurable). On the other hand, a model of the behavior of a system would not be particularly useful if it permitted instantaneous changes in state, hence the continuity requirement on trajectories (e.g. consider a robot moving instantaneously from one position to another).

2.1 Decision Variables

The input signal space $\mathcal{U}$ of all input signals and trajectory space $\mathcal{X}_{x_{0}}$ of all trajectories starting from the state $x_{0}$ are defined

[TABLE]

[TABLE]

where ${\rm Lip}(x)$ denotes the greatest lower bound222 Appendix A.3.2 defines greatest lower bound. on the set of Lipschitz constants333Appendix B.2.1 defines Lipschitz continuity. of $x$. It is important to note that the subscript $x_{0}$ in (2.1.2) is a parameter that can be varied to denote the space of trajectories originating from the initial condition $x_{0}$. Figure 2.1 illustrates a trajectory with Lipschitz constant $M$. Since the domains of functions in the sets (2.1.1) and (2.1.2) is variable, it will be useful to denote the terminal time of a trajectory’s or control signal’s domain by $\tau(x)$ or $\tau(u)$ so that the terminal state or terminal control input is given by $x(\tau(x))$ or $u(\tau(u))$ respectively.

The motivation for defining these two sets comes from the dynamical model of the system444Taking the derivative of (2.1.3) results in the more familiar diffrential form which is equivalent to the integral form. given in (2.1.3). The distinguishing feature between various system models is the function $f$ taking a state-control pair into $\mathbb{R}^{n}$. Throughout this thesis, the function $f$ is assumed to have a known global Lipschitz constant $L_{f}$ in its first argument, is measurable in its second, and is bounded by a constant $M$.

[TABLE]

A trajectory $x$ is called a solution to (2.1.3) for a given input signal $u\in\mathcal{U}$ and initial condition $x_{0}\in\mathbb{R}^{n}$ if it has the same time domain as $u$ and satisfies the equation for each $t$ in the domain of $u$. The measure $\mu$ will refer to the usual Lebesgue measure555A review of Lebesgue integration is presented in Appendix C. on $\mathbb{R}$. Let $\varphi_{x_{0}}\subset\mathcal{U}\times\mathcal{X}_{x_{0}}$ be a binary relation between the input signal space and trajectory space defined by $(u,x)$-pairs where $x$ is a solution to (2.1.3) with input signal $u$. Theorem 2.1.1 below states that the relation $\varphi_{x_{0}}$ is a function mapping $\mathcal{U}$ into $\mathcal{X}_{x_{0}}$. This will be important to the analysis presented in later chapters.

Theorem 2.1.1**.**

For each input signal in $\mathcal{U}$, there is a unique solution to (2.1.3) in $\mathcal{X}_{x_{0}}$.

The function $\varphi_{x_{0}}$ will be called the system map in light of this observation. This is a straight forward corollary to the standard existence-uniqueness theorem for integral equations provided in Appendix C.2.

2.2 Problem Specifications

We will denote the subset of trajectories which originate from a given initial condition $x_{ic}$ and remain within the set $X_{\mathrm{free}}$ by $\mathcal{X}_{\mathrm{free}}$.

[TABLE]

The subset of trajectories in $\mathcal{X}_{\mathrm{free}}$ which additionally terminate in $X_{\mathrm{goal}}$ is denoted $\mathcal{X}_{\mathrm{goal}}$.

[TABLE]

Figure 2.2 illustrates example trajectories in $\mathcal{X}_{\mathrm{goal}}$ and $\mathcal{X}_{\mathrm{free}}$.

The subset of input signals in $\mathcal{U}$ such that $\varphi_{x_{ic}}(u)\in\mathcal{X}_{\mathrm{free}}$ is denoted $\mathcal{U}_{\mathrm{free}}$. Similarly, the subset of input signals $\mathcal{U}$ such that $\varphi_{x_{ic}}(u)\in\mathcal{X}_{\mathrm{goal}}$ is denoted $\mathcal{U}_{\mathrm{goal}}$. It is important to remark that $\mathcal{U}_{\mathrm{free}}$ and $\mathcal{U}_{\mathrm{goal}}$ are the inverse images of $\mathcal{X}_{\mathrm{free}}$ and $\mathcal{X}_{\mathrm{goal}}$ under the system map $\varphi_{x_{ic}}$,

[TABLE]

The first part of the problem specification is to find a signal $u\in\mathcal{U}_{\mathrm{goal}}$.

Since there are often many input signals in $\mathcal{U}_{\mathrm{goal}}$, we additionally would like to minimize a performance objective. Performance objectives of the form

[TABLE]

are considered. The function $g$ from $X_{\mathrm{free}}\times\Omega$ into $(0,\infty)$ is a running cost which gives a rate of accumulation of cost for each state-control pair. It is assumed that a global Lipschitz constant $L_{g}$ for $g$ on $X_{\mathrm{free}}\times\Omega$ is known.

Since the state trajectory is completely determined by the input signal and initial condition, the cost functional $J$ is considered a mapping from $\mathcal{U}$ into $(0,\infty)$ parameterized by the initial condition. It will be convenient to extend the domain of $J_{x_{0}}$ with a symbol $\mathtt{NULL}$ where $J_{x_{0}}(\mathtt{NULL})=\infty$ for every $x_{0}$. We will also adopt the convention that $\inf_{u\in\emptyset}J_{x_{\mathrm{ic}}}(u)=\infty$.

Intuitively, we would like to minimize the cost $J$ over the set $\mathcal{U}_{\mathrm{goal}}$. However, a particular problem instance may not admit a minimizer or even a solution at all. Therefore, we will seek a solution to the following relaxed optimal kinodynamic motion planning problem:

Problem 2.2.1**.**

Given an initial condition $x_{ic}$, a set of admissible states $X_{\mathrm{free}}$, a set of goal states $X_{\mathrm{goal}}$, a set of admissible control inputs $\Omega$, a dynamic model $f$, and a running cost $g$; find a sequence $\{u_{R}\}\subset\mathcal{U}_{\mathrm{goal}}\cup\mathtt{NULL}$ such that

[TABLE]

An algorithm parameterized by a resolution $R\in\mathbb{N}$ whose output for each $R$ forms a sequence solving this problem will be called resolution complete.

2.3 Review of Assumptions

To summarize the problem formulation, the problem data for the optimal kinodynamic motion planning problem is $(x_{ic},X_{\mathrm{free}},X_{\mathrm{goal}},\Omega,f,g)$, where $x_{ic}$ is an initial condition, $X_{\rm free}$ is the set of admissible states, $X_{\mathrm{goal}}$ is the set of goal states, $f$ is the model of the system dynamics, and $g$ is the running cost. The assumptions on the problem data described in this section are:

1. A-1

The sets $X_{\mathrm{free}}$ and $X_{\mathrm{goal}}$ are open with respect to the topology induced by the Euclidean distance. 2. A-2

The set of admissible inputs $\Omega$ is bounded, and a bound $u_{\rm max}$ is known. 3. A-3

The running cost $g$ is strictly positive and Lipschitz continuous in both arguments with a known Lipschitz constant $L_{g}$. 4. A-4

The dynamic model $f$ is Lipschitz continuous in its first argument with a known Lipschitz constant $L_{f}$, Lebesgue measurable in its second argument, and bounded in both arguments by $M$.

There are two important things to consider regarding the assumptions of the input data to an algorithm or method. First, the applicability of a method requiring the assumptions to be met depends on how general the assumptions are. Secondly, given an instance of problem data, discerning whether it meets the assumptions should be an easy decision problem, decidable with an algorithm of lesser complexity than the algorithm the problem data is being given to. Otherwise, there is little value to the algorithm.

The formulation and assumptions of this chapter are quite general and can be discerned by inspection from typical problem data. For example, if $X_{\mathrm{free}}$ is described by the union of a finite set of closed polyhedra and $X_{\mathrm{goal}}$ is the union of a set of open polyhedra, then A-1 is satisfied. Similarly, robot actuators have clear design limitations bounding the set of admissible inputs so that A-2 is satisfied in all practical instances. Assumption A-3 is often verified by inspection of the running cost $g$. For example, minimum time problems with $g(z,w)=1$ are among the most frequently discussed objectives. The Lipschitz constant in this case is [math]. Lipschitz continuity of the dynamics is generally required for dynamical models to be well defined. Therefore, most models derived from physical laws will satisfy A-4. As a final remark on the generality of this formulation, it does not require any controllability properties of the dynamic model $f$ as is the case in [Li2016Asymptotically-, karaman2010optimal]. Although this makes the formulation more general it does not necessarily add to the practical value since there are few applications where motion planning is used on an uncontrollable system.

2.3.1 Verifiability of Assumptions

Most well known sampling-based motion planning algorithms impose an abstract assumption on the problem data for the algorithm to have the desired theoretical guarantees. In [kavraki1998analysis] the $\varepsilon$-goodness property was the basis of an analysis of the $\mathrm{PRM}$ algorithm. In [hsu1997path] the expansiveness property was required of the problem data to prove the probabilistic completeness of the $\mathrm{EST}$ algorithm. Similarly, in [RRT_Journal] the existence of an attraction sequence is required to prove the probabilistic completeness of the $\mathrm{RRT}$ algorithm. Lastly, in [karaman2011sampling], probabilistic completeness of the $\mathrm{RRT}^{*}$ algorithm relies on the problem data satisfying a $\delta$-robustness property.

While these assumptions are precisely defined, it is not clear how to verify if these properties are satisfied, whether this subset of problem instances are of practical value, or if there even exists a problem instance satisfying a particular assumption. These are important open problems in motion planning that have received little attention.

The following chapter uses techniques from the subject of topology to develop a foundation of tools for analyzing the optimal kinodynamic motion planning problem. A brief introduction to topology is presented in Appendix B.1.

To motivate the use of these techniques, we apply them to formally prove the intuitive fact that an open set of admissible states is sufficient for the $\delta$-robustness assumption of the $\mathrm{RRT}^{*}$ algorithm. The $\mathrm{RRT}^{*}$ algorithm requires that feasible problem instances be $\delta$-robustly feasible. That is, feasible problem instances must have a trajectory $x\in\mathcal{X}_{\mathrm{goal}}$, such that for some $\delta>0$ the trajectory satisfies

[TABLE]

Lemma 2.3.1**.**

If $X_{\mathrm{free}}$ is open, then every feasible problem instance ($\mathcal{X}_{\mathrm{goal}}\neq\emptyset$) is $\delta$-robustly feasible.

Proof.

Let $x$ be a trajectory in $\mathcal{X}_{\mathrm{goal}}$. Since $x$ is continuous, $x([0,\tau(x)])$ is a compact subset of $X_{\mathrm{free}}$ for all $t$ in the interval $[0,\tau(x)]$ (cf. Lemma B.1.5). By assumption, $X_{\mathrm{free}}$ is open so there exists a $\delta>0$ such that $B_{\delta}(x(t))\subset X_{\mathrm{free}}$ for all $t$ in $[0,\tau(x)]$ (cf. Corollary B.2.1) which is the definition of $\delta$-robust feasibility.

∎

Chapter 3 Topological Properties of the Input Signal and Trajectory Spaces

The problem formulation will use concepts from general topology to establish basic properties used by the $\mathrm{GLC}$ method.

The function spaces $\mathcal{X}_{x_{ic}}$ and $\mathcal{U}$ are equipped with metrics in order to perform an analysis on the subsets $\mathcal{X}_{\mathrm{free}}$, $\mathcal{X}_{\mathrm{goal}}$, $\mathcal{U}_{\mathrm{free}}$, and $\mathcal{U}_{\mathrm{goal}}$ in their respective metric topologies. It will follow from the assumption that $X_{\mathrm{free}}$ and $X_{\mathrm{goal}}$ are open in the standard topology on $\mathbb{R}^{n}$ that $\mathcal{X}_{\mathrm{free}}$ and $\mathcal{X}_{\mathrm{goal}}$ are open in the metric topology on $\mathcal{X}_{x_{ic}}$. We then review a result based on Gronwall’s inequality111Gronwall’s inequality is discussed in Appendix C. which shows $\varphi_{x_{ic}}$ is a continuous mapping from $\mathcal{U}$ into $\mathcal{X}_{x_{ic}}$. A direct consequence of these observations is $\mathcal{U}_{\mathrm{free}}$ and $\mathcal{U}_{\mathrm{goal}}$ are open subsets of $\mathcal{U}$ since the inverse image of an open set under a continuous function is open222This is the notion of continuity for functions on topological spaces. The relation to the definition for metric spaces is discussed in Lemma B.2.2.. The final important observation is the continuity of the cost function from $\mathcal{U}$ into $\mathbb{R}$.

These results will be used in later chapters as follows: A dense subset of $\mathcal{U}$ will be constructed in Chapter 4 as an approximation of $\mathcal{U}$. Since $\mathcal{U}_{\mathrm{free}}$ and $\mathcal{U}_{\mathrm{goal}}$ are open in $\mathcal{U}$, the approximation will also be dense in $\mathcal{U}_{\mathrm{free}}$ and $\mathcal{U}_{\mathrm{goal}}$. Next, the image of the approximation, intersected with $\mathcal{U}_{\mathrm{free}}$ and $\mathcal{U}_{\mathrm{goal}}$, under the cost function $J_{x_{ic}}$ will be dense in $J_{x_{ic}}(\mathcal{U}_{\mathrm{free}})$ and $J_{x_{ic}}(\mathcal{U}_{\mathrm{goal}})$ so that a signal, as will be seen, in the approximation of $\mathcal{U}_{\mathrm{goal}}$ achieves the optimal cost with arbitrarily high accuracy. Lastly, in Chapter 5 the continuity of the cost function is used once again to identify a large subset of the approximation which can be discarded without compromising the accuracy with which it approximates a signal with the optimal cost.

3.1 Metrics on the Signal and Trajectory Spaces

The input signal and trajectory spaces become metric spaces when equipped with the metrics $d_{\mathcal{U}}$ and $d_{\mathcal{X}_{x_{0}}}$ adapted from [yershov2011sufficient]:

[TABLE]

[TABLE]

Figures 3.1 and 3.2 illustrate these two metrics. Several results presented in this chapter regarding these metrics are simple variations of results in [yershov2011sufficient]. However, appropriately modified proofs are provided for the version of these results needed in later chapters.

While these distance functions have been utilized as metrics in the literature, verification that they satisfy the definition of a metric have not been published. These proofs are provided in Appendix E.

3.2 Continuity of the System Map $\varphi_{x_{0}}$ and Cost Functional $J_{x_{0}}$

A classical result in dynamical systems theory is the continuity of trajectories with respect to initial conditions. Lemma 3.2.1 is a restatement of this result with the appropriate notation.

Lemma 3.2.1**.**

Let $u$ be an input signals in $\mathcal{U}$; $x_{0},z_{0}$ initial conditions in $\mathbb{R}^{n}$; and $x_{1}\coloneqq\varphi_{x_{0}}(u)$, $x_{2}\coloneqq\varphi_{z_{0}}(u)$ the corresponding trajectories. Then the difference between these trajectories satisfies the inequality

[TABLE]

The standard proof can be found in Appendix C.2. Figure 3.3 illustrates the inequality in (3.2.1).

Lemma 3.2.2** ([yershov2011sufficient]).**

The system map $\varphi_{x_{0}}$ is continuous from $(\mathcal{U},d_{\mathcal{U}})$ into $(\mathcal{X}_{x_{0}},d_{\mathcal{X}})$.

Proof.

Let $u_{1},u_{2}$ be input signals in $\mathcal{U}$, ordered so that $\tau(u_{1})\leq\tau(u_{2})$. We will show that the map $\varphi_{x_{0}}$ is continuous at $u_{1}$. Denote trajectories $\varphi_{x_{0}}(u_{1})$ and $\varphi_{x_{0}}(u_{2})$ by $x_{1}$ and $x_{2}$ respectively. For $t$ in the time interval $[0,\tau(u_{1})]$, it follows from equation (2.1.3) that

[TABLE]

Then using the Lipschitz continuity of the dynamic model in A-4,

[TABLE]

Then by Gronwall’s inequality (cf. Lemma C.2.1),

[TABLE]

On the remaining time domain $[\tau(u_{1}),\tau(u_{2})]$, observe that $d_{\mathcal{U}}(u_{1},u_{2})<\delta$ implies $|\tau(u_{1})-\tau(u_{2})|<d_{\mathcal{U}}(u_{1},u_{2})/u_{max}$. Therefore,

[TABLE]

Then $d_{\mathcal{X}}(x_{1},x_{2})$ can be bounded as a function of $\delta$ by combining (3.2.4) and (3.2.5),

[TABLE]

Thus, the map $\varphi_{x_{0}}$ continuous at $u_{1}$ since for any $\varepsilon>0$, the signal $u_{2}$ can be chosen sufficiently close to $u_{1}$ so that $d_{\mathcal{X}}(x_{1},x_{2})<\varepsilon$, which is equivalent to the definition of continuity (cf. Lemma B.2.2). ∎

Note that the continuity is not uniform because of the appearance of $\tau(u_{1})$ in (3.2.6).

3.3 Properties of the Set of Solutions

The first important observation is that $\mathcal{X}_{\mathrm{free}}$ and $\mathcal{X}_{\mathrm{goal}}$ are open in the metric topology defined by $d_{\mathcal{X}}$ when $X_{\mathrm{free}}$ and $X_{\mathrm{goal}}$ are open in the standard topology.

Lemma 3.3.1** ([yershov2011sufficient]).**

$\mathcal{X}_{\mathrm{free}}$* is an open subset of $\mathcal{X}_{x_{ic}}$ in the topology induced by $d_{\mathcal{X}}$.*

Proof.

Either $\mathcal{X}_{\mathrm{free}}$ is empty, in which case it is open (cf. Appendix B.1), or it is non-empty. Assume the latter case and let $x_{1}$ be an element of $\mathcal{X}_{\mathrm{free}}$. Since $x_{1}$ is a Lipschitz continuous function from the compact interval $[0,\tau(x)]$ into $\mathbb{R}^{n}$, its image, $x_{1}([0,\tau(x)])$, is also compact (cf. Lemma B.1.5). Next, since $X_{\mathrm{free}}$ is open, and $x([0,\tau(x_{1})])$ is compact, there exists a $\rho>0$ such that $B_{\rho}(x_{1}(t))\subset X_{\mathrm{free}}$ for all $t\in[0,\tau(x_{1})]$ (cf. Corollary B.2.1). Now consider a trajectory $x_{2}$ in the ball of radius $\rho/2$ centered at $x_{1}$ in $\mathcal{X}_{x_{ic}}$ (note that this is in the function space $\mathcal{X}_{x_{ic}}$). Then, from the definition of $d_{\mathcal{X}}$,

[TABLE]

The first term in (3.3.1) implies, that on the interval $[0,\min\{\tau(x_{1}),\tau(x_{2})\}]$, the distance between $x_{1}(t)$ and $x_{2}(t)$ is within $\rho/2$. If $\tau(x_{2})\leq\tau(x_{1})$, then $x_{2}$ is necessarily in $\mathcal{X}_{\mathrm{free}}$. Now suppose that $\tau(x_{1})<\tau(x_{2})$. Then for $t>\tau(x_{1})$,

[TABLE]

The last term in (3.3.2) is further bounded by the Lipschitz constant for $x_{2}$,

[TABLE]

Thus, at any time $t$, $x_{2}(t)$ is within a distance of $\rho$ from some point in $x_{1}([0,\tau(x_{1})])$. Therefore, $x_{2}$ is in $\mathcal{X}_{\mathrm{free}}$. Since the choice of $x_{2}$ was arbitrary, $x_{1}$ is an interior point of $\mathcal{X}_{\mathrm{free}}$; since the choice of $x_{1}$ was arbitrary, $\mathcal{X}_{\mathrm{free}}$ is open.

∎

The trajectories discussed in the proof are illustrated in Figure 3.4. An analogous observation holds for the set $\mathcal{X}_{\mathrm{goal}}$.

Lemma 3.3.2** ([yershov2011sufficient]).**

$\mathcal{X}_{\mathrm{goal}}$* is an open subset of $\mathcal{X}_{x_{ic}}$ with respect to the topology induced by $d_{\mathcal{X}}$.*

Proof.

Either $\mathcal{X}_{\mathrm{goal}}$ is empty, in which case it is open, or it is non-empty. Assume the latter case and let $x_{1}$ be a trajectory in $\mathcal{X}_{\mathrm{goal}}$ in the metric subspace $(\mathcal{X}_{\mathrm{free}},d_{\mathcal{X}})$. Since $X_{\mathrm{goal}}$ is open, the terminal point $x_{1}(\tau(x_{1}))$ is an interior point of $X_{\mathrm{goal}}$, so there exists a $\rho>0$ such that $B_{\rho/2}(x_{1}(\tau(x_{1})))$ is a subset of $X_{\mathrm{goal}}$. Now consider a trajectory $x_{2}$ such that $d_{\mathcal{X}}(x_{1},x_{2})<\rho/2$. From the definition of $d_{\mathcal{X}}$,

[TABLE]

From (3.3.4), the difference in terminal times of $x_{1}$ and $x_{2}$ satisfies the bound

[TABLE]

Similarly, the distance between the states of the two trajectories at the shorter of the terminal times is bounded by

[TABLE]

These observations are then used to bound the distance between the terminal states of the two trajectories. The next step requires considering two cases: First, suppose $\tau(x_{2})\leq\tau(x_{1})$. Then

[TABLE]

Second, suppose $\tau(x_{1})<\tau(x_{2})$. The analogous application of the triangle inequality yields,

[TABLE]

Thus, the terminal state $x_{2}(\tau(x_{2}))$ is an element of $X_{\mathrm{goal}}$. Since the choice of $x_{2}$ was arbitrary, $x_{1}$ is an interior point of $\mathcal{X}_{\mathrm{goal}}$; since the choice of $x_{1}$ was arbitrary, every point in $\mathcal{X}_{\mathrm{goal}}$ is an interior point. Thus, $\mathcal{X}_{\mathrm{goal}}$ is open in the metric subspace $(\mathcal{X}_{\mathrm{free}},d_{\mathcal{X}})$. Since $\mathcal{X}_{\mathrm{free}}$ is open $\mathcal{X}_{x_{ic}}$, $\mathcal{X}_{\mathrm{goal}}$ is open in $\mathcal{X}_{x_{ic}}$ (cf. Lemma B.1.1). ∎

Now that we have established that the system map is continuous and $\mathcal{X}_{\mathrm{free}}$ and $\mathcal{X}_{\mathrm{goal}}$ are open subsets of its codomain, by definition of continuity, the inverse images of $\mathcal{X}_{\mathrm{free}}$ and $\mathcal{X}_{\mathrm{goal}}$ under the system map are open.

Theorem 3.3.1** ([yershov2011sufficient]).**

$\mathcal{U}_{\mathrm{free}}$* and $\mathcal{U}_{\mathrm{goal}}$ are open in the topology induced by $d_{\mathcal{U}}$.*

Proof.

By Lemma 3.2.1 the map $\varphi_{x_{ic}}$ is continuous, and by Lemmas 3.3.1 and 3.3.2, $\mathcal{X}_{\mathrm{free}}$ and $\mathcal{X}_{\mathrm{goal}}$ are open. The sets $\mathcal{U}_{\mathrm{free}}$ and $\mathcal{U}_{\mathrm{goal}}$ are defined as

[TABLE]

By the definition of continuity333Continuity is defined in Appendix B.1, $\mathcal{U}_{\mathrm{free}}$ and $\mathcal{U}_{\mathrm{goal}}$ are open. ∎

This is the essential property needed to prove the resolution completeness of the algorithm presented in later chapters.

Next, similar continuity properties for the cost function are derived. This continuity will be used to argue that the image of a dense subset of $\mathcal{U}$ will be dense in the cost space.

Lemma 3.3.3**.**

$J_{x_{0}}:\mathcal{U}\rightarrow\mathbb{R}$* is continuous for any $x_{0}\in\mathbb{R}^{n}$.*

Proof.

Let u_{1},u_{2}\in\mbox{\mathcal{U}} and, without loss of generality, assume $\tau(u_{1})\leq\tau(u_{2})$. Denote trajectories $\varphi_{x_{0}}(u_{1})$ and $\varphi_{x_{0}}(u_{2})$ by $x_{1}$ and $x_{2}$ respectively. The associated difference in cost is

[TABLE]

This is further bounded using the the Lipschitz constant of $g$ (cf. A-3)

[TABLE]

Since $x_{2}$ is continuous, $u_{2}$ is bounded, and $g$ is continuous, there exists a bound $G$ on $g(x_{2}(t),u_{2}(t))$ for $t\in[\tau(u_{1}),\tau(u_{2})]$. Thus, the difference in cost is further bounded by

[TABLE]

Observe that $|\tau(u_{2})-\tau(u_{1})|<d_{\mathcal{U}}(u_{1},u_{2})/u_{max}$ so the term $G|\tau(u_{1})-\tau(u_{2})|$ can be made arbitrarily small with $d_{\mathcal{U}}(u_{1},u_{2})$ sufficiently small. Similarly, since $\varphi_{x_{0}}$ is continuous, $d_{\mathcal{X}}(x_{1},x_{2})$ is made arbitrarily small with $d_{\mathcal{U}}(u_{1},u_{2})$ sufficiently small. Thus, for any $\varepsilon>0$, there exists a $\delta>0$ such that $d_{\mathcal{U}}(u_{1},u_{2})<\delta$ implies $|J_{x_{0}}(u_{1})-J_{x_{0}}(u_{2})<\varepsilon|$ which implies continuity by Lemma B.2.2. ∎

The cost functional is continuous with respect to the initial condition parameter as well. In fact, it is Lipschitz continuous which is used when comparing the cost of two trajectories to check if one of the trajectories is provably suboptimal.

Lemma 3.3.4**.**

For any $u\in\mathcal{U}$ and $x_{0},z_{0}\in\mathbb{R}^{n}$,

[TABLE]

Proof.

The absolute difference is bounded using the Lipschitz continuity of $g$. This is further bounded using (3.2.1). Denoting $x(t)=[\varphi_{x_{0}}(u)](t)$ and $z(t)=[\varphi_{z_{0}}(u)](t)$,

[TABLE]

∎

Chapter 4 Approximation of the Space of Input Signals

The goal of this thesis is to present an algorithm which computes an approximately optimal solution to Problem 2.2.1, and, since the decision variable is selected from $\mathcal{U}$, an uncountably infinite space, we will use a countably infinite approximation which can be systematically searched to provide an approximate solution with arbitrary accuracy in finite time.

The natural number $R$ will denote the resolution of the approximation, and the signal space $\mathcal{U}$ is approximated by a subset $\mathcal{U}_{R}$ indexed by the resolution. The approximation $\mathcal{U}_{R}$ of $\mathcal{U}$ is constructed from strings of a finite collection of primitive input signals. The primitive input signals each take a constant value from a finite subset $\Omega_{R}$ of $\Omega$ for a fixed duration. The approximation of the control inputs $\Omega$ can be any sequence of subsets whose dispersion converges to zero in $\Omega$. That is,

[TABLE]

The dispersion of a subset within a set is a measure of how well the subset approximates its containing set. Figure 4.1 illustrates the definition of dispersion appearing in equation (4.0.1). A family of subsets $\Omega_{R}$ exists and is often easily obtained with regular grids or random sampling for a given $\Omega$.

The primitive input signal $u:[0,1/R]\rightarrow\Omega$ associated to each $w$ in $\Omega_{R}$ is the following:

[TABLE]

The duration $1/R$ of the primitive input signals is selected for convenience. In general, all that is required is that the duration converges to zero with increasing $R$. For example, the duration of primitive input signals could alternatively be $1/R^{2}$ or $1/\log(R)$ with the appropriate adjustments made to the remaining analysis.

The approximation $\mathcal{U}_{R}$ is then all possible strings of input signals with length $h(R)$ or less where $h:\mathbb{N}\rightarrow\mathbb{N}$ is a horizon or depth limit. That is, an input signal in $\mathcal{U}_{R}$ will have a time domain $[0,d/R]$ for $d\leq h(R)$, and on each interval $\left[\frac{(i-1)}{R},\frac{i}{R}\right)$ with $i\in{1,...,d}$, the input signal takes one value from $\Omega_{R}$. Figure 4.2 illustrates the construction of signals in $\mathcal{U}_{R}$. The horizon limit can be any function satisfying

[TABLE]

This ensures that with sufficiently high resolution, the time domain of any input signal in $\mathcal{U}$ can be approximated by the time domain of a signal in $\mathcal{U}_{R}$. Figure 4.3 shows how increasing the resolution improves the available approximations in $\mathcal{U}_{R}$ to a particular input signal.

Terminology related to $\mathcal{U}_{R}$:

To simplify the discussion of how different input signals relate to one another, it will be useful to introduce some terminology. A parent of an input signal $w\in\mathcal{U}_{R}$ with domain $\left[0,i/R\right)$ is defined as the input signal $u\in\mathcal{U}_{R}$ with domain $\left[0,(i-1)/R\right)$ such that $w(t)=u(t)$ for all $t\in\left[0,\frac{c\cdot(i-1)}{R}\right)$. In this case, $w$ is a child of $u$. Two signals are siblings if they have the same parent. A tree (graph) is defined for each natural number $R$, with $\mathcal{U}_{R}$ as the vertex set, and edges defined by ordered pairs of signals $(u,w)$ such that $u$ is the parent of $w$. To serve as the root of the tree, $\mathcal{U}_{R}$ is augmented with the special input signal $Id_{\mathcal{U}}$ defined such that $J_{x_{0}}(Id_{\mathcal{U}})=0$ and $\tau(Id_{\mathcal{U}})=0$. $Id_{\mathcal{U}}$ has no parent, but is the parent of signals with domain $[0,1/R]$.

The signal $w$ is an ancestor of $u$ if $\tau(w)\leq\tau(u)$ and $w(t)=u(t)$ for all $t\in[0,\tau(w))$. In this case $u$ is a descendant of $w$. The depth of an input signal in $\mathcal{U}_{R}$ is the number of ancestors of that input signal.

Remark 1**.**

With these conventions, each signal is an ancestor, descendant, and sibling of itself.

4.1 Consistency of the Approximation

Since there is a metric on $\mathcal{U}$, a natural requirement of any approximation of this set by some subset is that for any fixed $u\in\mathcal{U}$,

[TABLE]

The interpretation of this condition is as follows: given any fixed111Note that this is a weaker condition than requiring the dispersion of $\mathcal{U}_{R}$ in $\mathcal{U}$ to converge to zero. input signal $u$ and $\varepsilon>0$, there is an input signal in $\mathcal{U}_{R}$ whose distance from $u$ is less than $\varepsilon$ with a sufficiently high resolution (i.e. sufficiently large $R$).

The next result establishes this property for $\mathcal{U}_{R}$.

Lemma 4.1.1**.**

For each $u\in\mathcal{U}$ and $\varepsilon>0$, there exists $R^{*}>0$ such that for any $R>R^{*}$ there exists $w\in\mathcal{U}_{R}$ satisfying $d_{\mathcal{U}}(u,w)<\varepsilon$.

Proof.

The proof will first rely on Lusin’s theorem222Lusin’s Theorem is discussed in Appendix C.1. for the existence of a continuous input signal approximating $u$. We then approximate the continuous input signal $u$ with an input signal $w$ in $\mathcal{U}_{R}$.

If follows directly from Lusin’s Theorem [lusin1912proprietes, feldman1981proof] that there exists a continuous input signal $\upsilon:[0,\tau(u)]\rightarrow\Omega$ such that

[TABLE]

Since $u$ and $\upsilon$ have the same time domain, the distance between the two is given by,

[TABLE]

Then from (4.1.2) this is equal to

[TABLE]

Since $\Omega$ is bounded, $\|u(t)-\upsilon(t)\|_{2}<2u_{max}$ resulting in the bound

[TABLE]

Next, we will construct a $w$ in $\mathcal{U}_{R}$ such that $d_{\mathcal{U}}(\upsilon,w)$ is small. The domain of $\upsilon$ is compact so the continuity is also uniform. Let $\delta(\epsilon)$ denote the modulus of continuity. That is,

[TABLE]

To construct an approximation of $\upsilon$ by $w\in\mathcal{U}_{R}$ choose $R$ sufficiently large so that

$\tau(\upsilon)<h(R)/R$, 2. 2.

$1/R<\min\left\{\frac{\varepsilon}{6u_{max}},\delta\left(\frac{\varepsilon}{6\tau(\upsilon)}\right)\right\}$, 3. 3.

there exists an integer $r<h(R)$ such that $0<\tau(\upsilon)-r/R<1/R$, 4. 4.

the dispersion of $\Omega_{R}$ in $\Omega$ is less than $\frac{\varepsilon}{6\tau(\upsilon)}$.

Note that if these conditions hold for $R^{*}$, they hold for all $R>R^{*}$.

It follows from (2.) and (4.) above, and the uniform continuity of $\upsilon$ that for each $i\in\{1,2,...,r\}$ and $t\in[(i-1)/R,i/R)$, there exists ${\rm v}_{i}\in\Omega_{R}$ such that $\|{\rm v}_{i}-\upsilon(t)\|_{2}<\frac{\varepsilon}{3\tau(\upsilon)}$. Select $w\in\mathcal{U}_{R}$ which is equal to ${\rm v}_{i}$ on each of these intervals so that $\tau(w)=r/R$, and $\tau(w)<\tau(\upsilon)$. By construction of $w$ we have

[TABLE]

Then by condition (2.) and (3.),

[TABLE]

Next, by construction of $w(t)$ we have

[TABLE]

Recall that $r/R<\tau(\upsilon)$ so that

[TABLE]

Integrating the right hand side of the inequality yields

[TABLE]

Thus, by the triangle inequality

[TABLE]

∎

The previous result showed that $\mathcal{U}_{R}$ consistently approximates $\mathcal{U}$ with respect to the metric $d_{\mathcal{U}}$. However, we are primarily interested in approximating $\mathcal{U}_{\mathrm{goal}}$. Since $\mathcal{U}_{\mathrm{goal}}$ is open with respect to $d_{\mathcal{U}}$, the set $\mathcal{U}_{R}\cap\mathcal{U}_{\mathrm{goal}}$333This is the set of signals within the approximation of $\mathcal{U}$ resulting in trajectories that satisfy obstacle avoidance constraints and terminal constraints. will inherit this property. We will combine this observation with the continuity of $J_{x_{ic}}$ to show that there exist signals in $\mathcal{U}_{R}\cap\mathcal{U}_{\mathrm{goal}}$ with cost arbitrarily close to the optimal cost:

Theorem 4.1.1**.**

For any $u\in cl\left(int\left(\mathcal{U}_{\mathrm{goal}}\right)\right)$ and $\varepsilon>0$ there exists $R^{*}>0$ such that for any $R>R^{*}$

[TABLE]

Proof.

Let $u$ be a signal in $cl\left(int\left(\mathcal{U}_{\mathrm{goal}}\right)\right)$. Then every neighborhood of $u$ has a nonempty intersection with $int(\mathcal{U}_{\mathrm{goal}})$. Equivalently, for every $\delta>0$,

[TABLE]

Let $\upsilon$ be an element of this intersection so that $\upsilon\in B_{\delta/2}(u)$ and $\upsilon\in int(\mathcal{U}_{\mathrm{goal}})$. Since $\upsilon$ is an interior point of $\mathcal{U}_{\mathrm{goal}}$, there exists $\rho>0$ such that $B_{\rho}(\upsilon)$ is a subset of $int(\mathcal{U}_{\mathrm{goal}})$. Now consider the neighborhood $B_{\min\{\rho,\delta/2\}}(\upsilon)$, which is a subset of $int(\mathcal{U}_{\mathrm{goal}})$. By Lemma 4.1.1, there exists an $R^{*}$ such that for all $R>R^{*}$, there is a signal $u$ in $\mathcal{U}_{R}$ satisfying $d_{\mathcal{U}}(w,\upsilon)<\min\{\rho,\delta/2\}$ which implies $w$ is in $\mathcal{U}_{\mathrm{goal}}$. Further,

[TABLE]

Now by the continuity of $J_{x_{\mathrm{ic}}}$, for $\delta$ sufficiently small, $d_{\mathcal{U}}(u,\upsilon)<\delta$ implies $|J_{x_{\mathrm{ic}}}(w)-J_{x_{\mathrm{ic}}}(u)|<\varepsilon$ from which the result follows. ∎

A sufficient condition for every $u\in\mathcal{U}_{\mathrm{goal}}$ to be contained in the closure of the interior of $\mathcal{U}_{\mathrm{goal}}$ is that $\mathcal{U}_{\mathrm{goal}}$ be open which is the case when Assumption A-1 is satisfied.

Chapter 5 The Generalized Label Correcting Method

Equipped with the set $\mathcal{U}_{R}$, one could enumerate the strings of input signal primitives in $\mathcal{U}_{R}$ to find the minimum cost input signal in $\mathcal{U}_{R}\cap\mathcal{U}_{\mathrm{goal}}$. Theorem 4.1.1 tells us that this procedure would solve Problem 2.2.1. However the number of signals in $\mathcal{U}_{R}$ is $h(R)^{{\rm card}(\Omega_{R})}$ which grows rapidly as the dispersion of the set of control inputs $\Omega_{R}$ in $\Omega$ converges to zero. This type of exhaustive search is analogous to searching over all paths in a graph for a shortest path between two vertices. However, this is an inefficient approach which is remedied with label correcting algorithms.

Label correcting algorithmms:

In a conventional label correcting method, the algorithm maintains the least cost path $p_{\rm label}$ known to terminate at each vertex of the graph. This path labels that vertex. At a particular iteration, if a path $p_{\rm new}$ under consideration does not have lower cost than the path labeling the terminal vertex, the path under consideration is discarded. Justification for this operation is that any extension $p_{\rm tail}$ of the path $p_{\rm new}$ which reaches the goal vertex can be also be used to extend $p_{\rm label}$ to reach the goal vertex. The cost of the extension $p_{\rm tail}$ in both cases will be the same, but the total cost of $p_{\rm new}$ concatenated with $p_{\rm tail}$ will certainly be no less than the total cost of $p_{\rm label}$ concatenated with $p_{\rm tail}$. Therefore, there is no need to consider any extensions of $p_{\rm new}$. As a consequence, the subtree of paths originating from the discarded path will not be evaluated.

Generalizing the notion of a label:

Observe that the label of a vertex in conventional label correcting algorithms is in fact a label for the paths terminating at that vertex. Then each vertex identifies an equivalence class of paths which terminate at that vertex. Paths within each equivalence class are ordered by their cost, and the efficiency of label correcting methods comes from narrowing the search to minimum cost paths in their associated equivalence class. The generalization is to identify paths associated to trajectories terminating in the same region of the state space instead of the same state. In the context of trajectory planning, this is complicated by the fact that the optimal cost from any particular state to the goal may be discontinuous. This makes it difficult to prove that a trajectory terminating close to another trajectory with lesser cost can be discarded. In this chapter we derive precise conditions for discarding a trajectory that resolves this issue.

5.1 Constructing Equivalence Classes of Signals

The equivalence classes of input signals are induced by a partition of $X_{\mathrm{free}}$. A partition of $X_{\mathrm{free}}$ is said to have radius $r$ if each element of the partition is a set contained in a neighborhood of radius $r$. No further assumptions on the geometry of these sets will be required. A partition of radius $r$ is illustrated in Figure 5.1

For now we only consider hypercube partitions whose radius is controlled by a function $\eta:\mathbb{N}\rightarrow\mathbb{R}_{>0}$. For states $p_{1},p_{2}\in\mathbb{R}^{n}$ we write $p_{1}\overset{R}{\sim}p_{2}$ if

[TABLE]

where $\left\lfloor\cdot\right\rfloor$ is the coordinate-wise floor map (e.g. $\left\lfloor(2.9,3.2)\right\rfloor=(2,3)$). The equivalence classes of the $\overset{R}{\sim}$ relation define a simple hypercube partition of radius $\sqrt{n}/\eta(R)$. We extend this relation to control inputs by comparing the terminal state of the resulting trajectory. For $u_{1},u_{2}\in\mathcal{U}_{R}$ we write $u_{1}\overset{\mathcal{U}_{R}}{\sim}u_{2}$ if the resulting trajectories terminate in the same hypercube. That is,

[TABLE]

Figure 5.2 illustrates the intuition behind this equivalence relation.

The principal contribution of this thesis is the GLC conditions for ordering input signals among the equivalence classes of the relation $\overset{\mathcal{U}_{R}}{\sim}$. The $\mathrm{GLC}$ conditions define a partial ordering $\prec_{R}$ on $\mathcal{U}_{R}$ which is used to identify signals which can be discarded. We write $u_{1}\prec_{R}u_{2}$ if:

1. GLC-1

$u_{1}\overset{\mathcal{U}_{R}}{\sim}u_{2}$, 2. GLC-2

$\tau(u_{1})\leq\tau(u_{2}),$ 3. GLC-3

$J_{x_{\mathrm{ic}}}(u_{1})\leq J_{x_{\mathrm{ic}}}(u_{2}).$

That is, $u_{1}$ is $\prec_{R}$-less than $u_{2}$ if they result in trajectories terminating within the same region of the states space (GLC-1), the duration of $u_{1}$ is no greater than the duration of $u_{2}$ (GLC-2), and the cost of $u_{1}$ is less than the cost of $u_{2}$ (GLC-3). These conditions are sharper than the ones presented in [paden2016generalized] where GLC-3 included a threshold for the difference in cost between $u_{1}$ and $u_{2}$.

A signal $u_{1}$ is called minimal if there is no $u_{2}\in\mathcal{U}_{R}$ such that $u_{2}\prec_{R}u_{1}$. Such a signal can be thought of as being a good candidate for later expansion during the search. Otherwise, it can be discarded. In order for the $\mathrm{GLC}$ method to be a resolution complete algorithm, the scaling parameter $\eta$ must satisfy

[TABLE]

Figure 5.3 contrasts the trajectories resulting from minimal signals in $\mathcal{U}_{R}$ with an exhaustive enumeration of $\mathcal{U}_{R}$ for a two dimensional kinematic point robot. The minimal signals result in approximately optimal trajectories that uniformly cover the free space $X_{\mathrm{free}}$ while exhaustively enumerating $\mathcal{U}_{R}$ requires many more trajectories to be evaluated.

5.2 Properties of the $\mathrm{GLC}$ Conditions

This section will develop a number of important concepts and basic properties of the partial order $\prec_{R}$.

The $\epsilon$-interior of the set $\mathcal{X}_{\mathrm{free}}$ and $\mathcal{X}_{\mathrm{goal}}$, are defined by

[TABLE]

The inverse image of these sets under the system map $\varphi_{x_{ic}}$ is denoted

[TABLE]

Note that if $x$ is an element of $\mathcal{X}_{\mathrm{free}}^{\epsilon}$, then $B_{\epsilon}(x(t))\subset X_{\mathrm{free}}$. The same is true for $\mathcal{X}_{\mathrm{goal}}$.

To simplify notation we will denote the optimal cost of signals in $\mathcal{U}_{\mathrm{goal}}^{\epsilon}\cap\mathcal{U}_{R}$ by $c_{R}^{\epsilon}$,

[TABLE]

Like, the optimal cost $c^{*}$ and the approximate optimal cost $c_{R}$, $c_{R}^{\epsilon}$ can, in some cases, be $\infty$. An intuitive, but important property regarding the cost $c_{R}^{\varepsilon}$ is that it converges to the optimal cost $c^{*}$ as $\varepsilon$ tends to [math] and $R$ tends to $\infty$.

Lemma 5.2.1**.**

If $\lim_{R\rightarrow\infty}\epsilon(R)=0$, then $\lim_{R\rightarrow\infty}c_{R}^{\epsilon(R)}=c^{*}$.

Proof.

Since the infimum in (2.2.5) may not be attained, the first step is to identify an input signal which is arbitrarily close to the optimal cost. By the definition of $c^{*}$ in (2.2.5), for any $\varepsilon>0$ there exists $\omega\in\mathcal{U}_{\mathrm{goal}}$ such that $J_{x_{\mathrm{ic}}}(\omega)-\varepsilon/2<c^{*}$.

Next, we use the topological properties discussed in Chapter 3 to construct a neighborhood of $\omega$ containing signals in $\mathcal{U}_{\mathrm{goal}}$. Since $\mathcal{U}_{\mathrm{goal}}$ is open and $\varphi_{x_{\mathrm{ic}}}$ is continuous, there exists $\tilde{r}>0$ and $\rho>0$ such that $B_{\tilde{r}}(\omega)\subset\mathcal{U}_{\mathrm{goal}}$ and $\varphi_{x_{\mathrm{ic}}}(B_{\tilde{r}}(\omega))\subset B_{\rho}(\varphi_{x_{\mathrm{ic}}}(\omega))$. Thus, $\omega\in\mathcal{U}_{\mathrm{goal}}^{\rho}$. From the continuity of $J$ in Lemma 3.3.3 there also exists a positive $\delta<r$ such that for any signal $\upsilon$ with $d_{\mathcal{U}}(\upsilon,\omega)<\delta$ we have $|J_{x_{\mathrm{ic}}}(\omega)-J_{x_{\mathrm{ic}}}(\upsilon)|<\varepsilon/2$.

The next step is to find a feasible signal in $\mathcal{U}_{R}$ which closely approximates $\upsilon$. Choose $R^{*}$ to be sufficiently large such that $R>R^{*}$ implies \mbox{\epsilon}(R)<\rho and $B_{\delta}(\omega)\cap\mathcal{U}_{R}\neq\emptyset$. Such a resolution $R^{*}$ exists by Theorem 4.1.1 and the assumption $\lim_{R\rightarrow\infty}\epsilon(R)=0$. Now choose $u\in B_{\delta}(\omega)\cap\mathcal{U}_{R}$. Then $|J_{x_{\mathrm{ic}}}(u)-J_{x_{\mathrm{ic}}}(\omega)|<\varepsilon/2$ and $u\in\mathcal{U}_{\mathrm{goal}}^{\rho}\subset\mathcal{U}_{\mathrm{goal}}^{\epsilon(R)}$.

Finally, we use the triangle inequality to show that $u$ has nearly the optimal cost. Then, by definition of $c_{R}^{\epsilon(R)}$, $u\in\mathcal{U}_{\mathrm{goal}}^{\epsilon(R)}$ implies $c_{R}^{\epsilon(R)}\leq J_{x_{\mathrm{ic}}}(u)$. Finally, by the triangle inequality,

[TABLE]

Rearranging the expression yields $J_{x_{\mathrm{ic}}}(u)<c^{*}+\varepsilon$ and thus, $c_{R}^{\epsilon(R)}<c^{*}+\varepsilon$. The result follows since the choice of $\varepsilon$ is arbitrary. ∎

Some new notation:

At this point it will be advantageous to introduce a concatenation operation on elements of $\mathcal{U}$ and $\mathcal{X}_{x_{0}}$. For $s_{1},s_{2}\in\mathcal{U}\cup\mathcal{X}_{x_{0}}$, their concatenation $s_{1}s_{2}$ is defined by

[TABLE]

The concatenation is illustrated in Figure 5.4.

The concatenation operation will be useful together with the following equalities for $u_{1},u_{2}\in\mathcal{U}$,

[TABLE]

[TABLE]

Equations (5.2.6) and (5.2.7) follow directly from (2.1.3). The interpretation of (5.2.6) is that the cost of $u_{1}$ concatenated with $u_{2}$ is equal to the cost of executing the control signal $u_{1}$ from $x_{0}$ plus the cost of executing $u_{2}$ from the terminal state of the trajectory resulting from $u_{1}$.

The next result can be interpreted as a generalization of Bellman’s principle of optimality [bellman1956dynamic] and is the basis for the generalized label correcting method in the same way that the principle of optimality is the basis for label correcting methods. Figure 5.5 illustrates the statement of the next theorem.

Theorem 5.2.1** (Principle of Optimality).**

Let $\gamma=\frac{\sqrt{n}}{\eta(R)}\frac{L_{g}}{L_{f}}\left(e^{L_{f}\left(\frac{h(R)}{R}-\tau(u_{j})\right)}-1\right)$, and $\delta=\frac{\sqrt{n}}{\eta(R)}e^{L_{f}\left(\frac{h(R)}{R}-\tau(u_{j})\right)}$. If $u_{i},u_{j}\in\mathcal{U}_{R}\cap\mathcal{U}_{\mathrm{free}}$ and satisfy $u_{i}\prec_{R}u_{j}$, then for each descendant of $u_{j}$ in $\mathcal{U}_{R}\cap\mathcal{U}_{\mathrm{goal}}^{\delta}$ with cost $c_{j}$, there exists a descendant of $u_{i}$ in $\mathcal{U}_{R}\cap\mathcal{U}_{\mathrm{goal}}$ with cost $c_{i}\leq c_{j}+\gamma$.

Proof.

Suppose there exists $w\in\mathcal{U}_{R}$ such that $u_{j}w\in\mathcal{U}_{R}\cap\mathcal{U}^{\delta}$. By GLC-1 the signals satisfy $u_{i}\overset{\mathcal{U}_{R}}{\sim}u_{j}$ which means

[TABLE]

Note that $\tau(w)\leq h(R)/R-\tau(u_{j})$ since $u_{j}w$ has depth no greater than $h(R)$. Then, by Lemma (3.2.1), for all $t\in[0,\tau(w)]$,

[TABLE]

Thus, $u_{i}w\in\mathcal{U}_{\mathrm{goal}}.$ Next, we show that $c_{i}\leq c_{j}+\gamma$. From equation (5.2.6),

[TABLE]

Combining the continuity of the cost functional with respect to the initial condition (Lemma 3.3.4) and equation (5.2.8) yields

[TABLE]

Then combining (5.2.10) and (5.2.11) yields

[TABLE]

Therefore $c_{i}\leq c_{j}+\gamma$. ∎

In reference to the above theorem, since $u_{j}w\in\mathcal{U}_{R}$ which is limited to signals with depth $h(R)$, the signal $w$, which is concatenated with $u_{i}$ to form a signal in $\mathcal{U}_{\mathrm{goal}}\cap\mathcal{U}_{R}$, necessarily satisfies

[TABLE]

where $\mathtt{depth}$ is the depth of a particular signal.

5.3 A $\mathrm{GLC}$ Algorithm

Pseudocode for a $\mathrm{GLC}$ algorithm is defined in Algorithm 1 below. As the name suggests, it is very similar to a canonical label correcting algorithm. A set $Q$ serves as a priority queue of candidate signals. A set $\Sigma$ contains signals representing labels of $\overset{\mathcal{U}_{R}}{\sim}$ equivalence classes.

The method $\texttt{expand}(u)$ returns the set of all children of $u$. The method $\texttt{pop}(Q)$ deletes from Q, and returns an input signal $\hat{u}$ such that

[TABLE]

so that the presented $\mathrm{GLC}$ algorithm is a best-first search111Like canonical label correcting methods, there are many variations that utilize alternative orderings. For example, the addition of an admissible heuristic [hart1968formal] in (5.3.1) can be used to guide the search without affecting the solution accuracy. Admissible heuristics for kinodynamic motion planning are discussed in detail in Chapter 6.. The method $\texttt{find}(u,\Sigma)$ returns $w\in\Sigma$ such that $u\overset{R}{\sim}w$ or $\mathtt{NULL}$ if no such $w$ is present in $\Sigma$. Problem specific collision and goal checking subroutines are used to evaluate $u\in\mathcal{U}_{\mathrm{feas}}$ and $u\in\mathcal{U}_{\mathrm{goal}}$. The method $\texttt{depth}(u)$ returns the number of ancestors of $u$.

The algorithm begins by adding the root $Id_{\mathcal{U}}$ to the queue (line 1), and then enters a loop which recursively removes and expands the top of the queue (line 3) adding children to $S$ (line 4). If the queue is empty the algorithm terminates (line 2) returning $\mathtt{NULL}$ (line 14). Each signal in $S$ (line 5) is checked for membership in $\mathcal{U}_{\mathrm{goal}}$ in which case the algorithm terminates returning a feasible solution with approximately the optimal cost. Otherwise, the signals are checked for infeasibility or suboptimality by the $\mathrm{GLC}$ conditions (line 9). Next, a relabeling condition for the associated equivalence classes (i.e. grid cells) of remaining signals is checked (line 11). Finally, remaining signals in $S$ are added to the queue (line 13).

In addition to the problem data, the algorithm requires two functions $h(R)$ and $\eta(R)$ satisfying equations (4.0.3) and (5.1.3), and the control inputs $\Omega_{R}$ satisfying (4.0.1).

5.4 Proof of Resolution Completeness

The goal of this section is to prove that Algorithm 1 is resolution complete for Problem 2.2.1. Algorithm 1 is very similar to classical label correcting algorithms and the proof parallels standard proofs of completeness for these methods. The reader may benefit from reviewing the proof in the simpler case where the algorithm is applied to a shortest path problem in a graph (e.g. [bertsekas1995dynamic, chapter 2] or Appendix D).

In Theorem 5.2.1, the quantity $\delta$ was constructed based on: the radius of the partition of $\mathcal{X}_{\mathrm{free}}$, the sensitivity of solutions to initial conditions determined by Lemma 3.2.1, and the maximum duration of signals in $\mathcal{U}_{R}$. In the proof of the next result, we will recursively apply Theorem 5.2.1 and it will be useful to define the following sum that arises when repeatedly applying Theorem 5.2.1,

[TABLE]

When $k=h(R)$, the sum satisfies the inequality

[TABLE]

which is derived in Appendix E.3. Similarly, it will be convenient to define the quantity $\gamma_{k}$ related to $\gamma$ of Theorem 5.2.1,

[TABLE]

which is bounded by

[TABLE]

An important property of the right hand side of (5.4.2) and (5.4.4) is that they converge to zero when $\eta(R)$ satisfies equation (5.1.3). Thus, $\delta_{h(R)}$ and $\gamma_{h(R)}$ converge to zero as $R$ tends to infinity.

The pruning operation in lines 9-10 of Algorithm 1, in a sense, discards candidate input signals as liberally as possible. In the next theorem, it is shown that a signal in $\mathcal{U}_{\mathrm{goal}}^{\delta_{h(R)}}$ with cost less than $c_{R}^{\delta_{h(R)}}+\gamma_{h(R)}$ is eventually evaluated by the algorithm in line 4 despite this pruning operation.

Theorem 5.4.1**.**

The $\mathrm{GLC}$ method described by Algorithm 1 terminates in finite time and returns a solution with cost less than or equal to $c^{\delta_{h(R)}}_{R}+\gamma_{h(R)}$.

Proof.

(Finite running time) The queue is a subset of $\mathcal{U}_{R}\cup\{Id_{\mathcal{U}}\}$ and at line 3 in each iteration a lowest cost signal $u$ is removed from the queue. In line 13, only children of the current signal $u$ are added to the queue. Since $\mathcal{U}_{R}$ is organized as a tree and has no cycles, any signal $u$ will enter the queue at most once. Therefore the queue must be empty after a finite number of iterations so the algorithm terminates.

(Approximate Optimality) Next, consider as a point of contradiction the hypothesis that the output has cost greater than $c^{\delta_{h(R)}}_{R}+\gamma_{h(R)}$. Then it is necessary that $c^{\delta_{h(R)}}_{R}+\gamma_{h(R)}<\infty$, and by the definition of $c^{\delta_{h(R)}}_{R}$ in (5.2.3), it is also necessary that $\mathcal{U}_{R}\cap\mathcal{U}_{\mathrm{goal}}^{\delta_{h(R)}}$ is non-empty.

Choose $u^{*}\in\mathcal{U}_{R}\cap\mathcal{U}_{\mathrm{goal}}^{\delta_{h(R)}}$ with cost $J_{x_{\mathrm{ic}}}(u^{*})=c_{R}^{\delta_{h(R)}}$. It follows from the contradiction hypothesis that $u^{*}$ does not enter the queue. Otherwise, by (5.3.1) it would be evaluated before any signal of cost greater than $c_{R}^{\delta_{h(R)}}$ and the algorithm would terminate returning this input signal. If $u^{*}$ does not enter the queue, then a signal $u_{0}$ must at some iteration be present in $\Sigma$ which prunes an ancestor $a_{0}$ of $u^{*}$ ($u_{0}\prec_{R}a_{0}$ in line 9). This ancestor must satisfy $\texttt{depth}(a_{0})>0$ since the ancestor with depth [math] is $Id_{\mathcal{U}}$ which enters queue in line 1. By Theorem 5.2.1, $u_{0}$ has a descendant of the form $u_{0}d_{0}\in\mathcal{U}_{\mathrm{goal}}^{\delta_{h(R)}-\delta_{0}}$ and $J_{x_{\mathrm{ic}}}(u_{0}d_{0})\leq c_{R}^{\delta_{h(R)}}+\gamma_{0}$. Additionally, $\mathtt{depth}(a_{0})>0$ implies $\mathtt{depth}(d_{0})\leq h(R)-1$ by equation (5.2.13).

Having pruned $u^{*}$ in lines 9-10, the signal $u_{0}\in\Sigma$, or a sibling which prunes $u_{0}$ (and by the transitivity of $\prec_{R}$, prunes $u^{*}$) must at some point be present in the queue (cf. line 12-13). Of these two, denote the one that ends up in the queue by $\tilde{u}_{0}$. Since $\tilde{u}_{0}$ is at some point present in the queue and $\tilde{u}_{0}d_{0}\in\mathcal{U}_{\mathrm{goal}}^{\delta_{h(R)}-\delta_{0}}$, a signal $u_{1}\in\Sigma$ must prune an ancestor $a_{1}$ of $\tilde{u}_{0}d_{0}$ ($u_{1}\prec_{R}a_{1}$ in lines 9-10). Since $\tilde{u}_{0}$ is at some point present in the queue, the ancestor $a_{1}$ of $\tilde{u}_{0}d_{0}$, must have greater depth than $\tilde{u}_{0}$. By Theorem 5.2.1, $u_{1}$ has a descendant of the form $u_{1}d_{1}\in\mathcal{U}_{\mathrm{goal}}^{\delta_{h(R)}-\delta_{1}}$ and $J_{x_{\mathrm{ic}}}(u_{1}d_{1})\leq c_{R}^{\delta_{h(R)}}+\gamma_{1}$. Additionally, $\mathtt{depth}(a_{1})>\tilde{u}_{0}$ implies $\mathtt{depth}(d_{1})\leq\mathtt{depth}(d_{0})-1\leq h(R)-2$ by equation (5.2.13).

Continuing this line of deduction leads to the observation that a signal $u_{h(R)-1}$, with a descendant of the form $u_{h(R)-1}d_{h(R)-1}\in\mathcal{U}_{\mathrm{goal}}^{\delta_{h(R)}-\delta_{h(R)-1}}$ and $J_{x_{\mathrm{ic}}}(u_{h(R)-1}d_{h(R)-1})\leq c_{R}^{\delta_{h(R)}}+\gamma_{h(R)-1}$, will be present in the queue; and $\mathtt{depth}(d_{h(R)-1})\leq 1$. Since $u_{h(R)-1}$ is at some point present in the queue, a signal $u_{h(R)}\in\Sigma$ must prune an ancestor $a_{h(R)}$ of $u_{h(R)-1}d_{h(R)-1}$ ($u_{h(R)}\prec_{R}a_{h(R)}$ in lines 9-10). Since $u_{h(R)-1}$ is at some point present in the queue, the ancestor $a_{h(R)}$ of $u_{h(R)-1}d_{h(R)-1}$, must have greater depth than $u_{h(R)-1}$, and therefore, is equal to $u_{h(R)-1}d_{h(R)-1}$. Thus, $u_{h(R)}\in\mathcal{U}_{\mathrm{goal}}^{\delta_{h(R)}-\delta_{h(R)-1}}$ and $J(u_{h(R)})\leq c_{R}^{\delta_{h(R)}}+\gamma_{h(R)}$. Then $u_{h(R)}$ or a sibling which prunes $u_{h(R)}$ will be added to the queue; a contradiction of the hypothesis since this signal will be removed from the queue and the algorithm will terminate, returning this signal in line 7.

∎

The choice of $\delta_{h(R)}$ and $\gamma_{h(R)}$ in Theorem 5.4.1 converge to zero as $R$ tends to infinity by (5.1.3). Then by Lemma 5.2.1 we have $c_{R}^{h(R)}+\gamma_{h(R)}\rightarrow c^{*}$. An immediate corollary is that the $\mathrm{GLC}$ method is resolution complete. It is important to note that for low resolution $R$, it is possible that $c_{R}^{h(R)}=\infty$.

Corollary 5.4.1**.**

Let $w_{R}$ be the signal returned by the $\mathrm{GLC}$ method for resolution $R$. Then $\lim_{R\rightarrow\infty}J_{x_{ic}}(w_{R})=c^{*}$. That is, the $\mathrm{GLC}$ method is a resolution complete algorithm for the optimal kinodynamic motion planning problem.

5.5 Kinodynamic Motion Planning Examples

The $\mathrm{GLC}$ method (Algorithm 1) was tested on five problems and compared, when applicable, to the implementation of $\mathrm{RRT}^{*}$ from [rrt_implementation] and $\mathrm{SST}$ from [BBekris2015]. The goal is to examine the performance of the $\mathrm{GLC}$ method on a wide variety of problems. The examples include under-actuated nonlinear systems, multiple cost objectives, and environments with/without obstacles. Note that adding obstacles effectively speeds up the $\mathrm{GLC}$ method since it reduces the size of the search tree.

Another focus of the examples is on real-time application. In each example the running time for $\mathrm{GLC}$ method to produce a (visually) acceptable trajectory is comparable to the execution time. Of course this will vary with problem data and computing hardware.

Implementation Details:

The $\mathrm{GLC}$ method was implemented in C++ and run with a 3.70GHz Intel Xeon CPU. The set $Q$ was implemented with an STL priority queue so that the $\texttt{pop}(Q)$ method and insertion operations have logarithmic complexity in the size of $Q$. The set $\Sigma$ was implemented with an STL set which uses a binary search tree so that $\texttt{find}(w,\Sigma)$ also has logarithmic complexity in the size of $\Sigma$.

Sets $X_{\mathrm{free}}$ and $X_{\mathrm{goal}}$ are described by algebraic inequalities. The approximation of the input space $\Omega_{R}$ is constructed by uniform deterministic sampling of $R^{m}$ (i.e. the resolution $R$ raised to the power $m$ which is the dimension of the input space) controls from $\Omega$. Recall $m$ is the dimension of the input space.

Evaluation of $u\in\mathcal{U}_{\mathrm{feas}}$ and $u\in\mathcal{U}_{\mathrm{goal}}$ is approximated by first numerically computing $\varphi_{x_{\mathrm{ic}}}(u)$ with Euler integration (except for $\mathrm{RRT}^{*}$ which uniformly samples along the local planning solution). The number of time-steps is given by $N=\left\lceil\tau(u)/\Delta\right\rceil$ with duration $\tau(u)/N$. Maximum time-steps $\Delta$ are 0.005 for the first problem, 0.1 for the second through fourth problem, and 0.02 for the last problem. Feasibility is then approximated by collision checking at each time-step along the trajectory.

An additional scaling of the input signal primitive duration by $\gamma$ is introduced in these examples. Theoretically, the output of the algorithm for any value of $\gamma$ will be consistent with Theorem 5.4.1, but in practice it is advantageous to select a scaling $\gamma$ representing a characteristic time-scale for the problem. For example, if the motion is expected to take on the order of microseconds, then selecting $\gamma=10^{-6}$ would adjust the time-scale of the search appropriately.

5.5.1 Shortest Path Problem

A shortest path problem in $\mathbb{R}^{n}$ can be represented by the dynamic model

[TABLE]

the running-cost

[TABLE]

and the control input space

[TABLE]

In this example, $X_{\mathrm{free}}$ and $X_{\mathrm{goal}}$ are described by polyhedral sets illustrated in Figure 5.6.

The parameters for the $\mathrm{GLC}$ method are summarized in Table 5.1. Note that $h(R)$ and $\eta(R)$ satisfy the asymptotic constraints (4.0.3) and (5.1.3).

The $\mathrm{GLC}$, $\mathrm{SST}$, and $\mathrm{RRT}^{*}$ methods were all tested in this example. The average performance from 10 trials is reported for the $\mathrm{SST}$ and $\mathrm{RRT}^{*}$ methods. Figure 5.6 summarizes the results. This example also considers a guided search using the cost of the $\mathrm{RRT}^{*}$ local planning subroutine solution to the goal as a heuristic.

This is the only example in this section where the exact solution to the problem is known. In this case we can compare the relative convergence rates of the $\mathrm{GLC}$ method, $\mathrm{SST}$, and $\mathrm{RRT}^{*}$.

5.5.2 Torque-Limited Pendulum Swing-Up

A minimum-time torque-limited pendulum swing-up problem can be represented by the dynamic model

[TABLE]

with the running cost

[TABLE]

and the control input space

[TABLE]

The free space and goal set are described by

[TABLE]

The initial state is the origin $(\theta(0),\omega(0))=(0,0)$.

The parameters for the $\mathrm{GLC}$ method are summarized in Table 5.1.

The $\mathrm{GLC}$ and $\mathrm{SST}$ methods were tested in this example with the average performance from 10 trials reported for the $\mathrm{SST}$ method. Figure 5.7 summarizes the results.

5.5.3 Torque Limited Acrobot Swing-Up

The acrobot is a double link pendulum actuated at the middle joint. The expression for the four dimensional system dynamics are cumbersome to describe and we refer to [spong1995swing] for the details. The model parameters, free space, and goal region are identical to those in the benchmark provided in [BBekris2015] with the exception that the radius of the goal region is reduced from $2.0$ to $0.5$. A minimum-time running-cost

[TABLE]

is used. The control input space is $\Omega=[-4.0,4.0]$ representing the minimum and maximum torque that can be applied at the middle joint.

The parameters for the $\mathrm{GLC}$ method are summarized in Table 5.3.

The $\mathrm{GLC}$ and $\mathrm{SST}$ methods were tested in this example with the average performance from 10 trials reported for the $\mathrm{SST}$ method. Figure 5.8 summarizes the results.

5.5.4 Thrust Limited 3D Point Robot

To emulates the mobility of an agile aerial vehicle (e.g. a quadrotor with high bandwidth attitude control), the system dynamics are modeled by

[TABLE]

where $x(t)$, $v(t)$, and $u(t)$ are each elements of $\mathbb{R}^{3}$; there are a total of six states and three control inputs. The quadratic dissipative force anti-parallel to the velocity $v(t)$ models aerodynamic drag during high speed flight. The minimum time running cost is

[TABLE]

and the input space is

[TABLE]

The planning task is for the point robot to navigate from an initial state in one room to a destination in an adjacent room connected by a small window. This free space is illustrated in Figure 5.9. The dark blue marker in the leftmost room illustrates the goal configuration, and the light blue marker in the rightmost room illustrates the starting configuration. The velocity is initially zero, and the terminal velocity is left free. A guided search is also considered with heuristic given by the distance to the goal divided by the maximum speed $v$ of the robot (A maximum speed of $\sqrt{{50}}$ can be determined from the dynamics and input constraints). The parameters for the $\mathrm{GLC}$ method are summarized in Table 5.1.

The control input approximation $\Omega_{R}$ is generated by an increasing number of points distributed uniformly on $\Omega$. However, because of the symmetry of the sphere, the computational procedure for generating $\Omega_{R}$ in this case produces the same set of points up to a random orthogonal transformation [paden2017]. Since there is some randomness in this example, the results reported in Figure 5.10 are averaged over 10 trials both for the $\mathrm{GLC}$ and $\mathrm{SST}$ algorithm.

Figure 5.9 shows the three outputs of the $\mathrm{GLC}$ algorithm at three different resolutions to illustrate the improvement in solution quality with increasing resolution.

5.5.5 Nonholonomic Wheeled Robot

The dynamic model emulating the mobility of a wheeled robot is given by

[TABLE]

A running cost which penalizes a combination of time and lateral acceleration (believed to be correlated with rider comfort) is given by

[TABLE]

The input space, which is consistent with a turning radius of $1$, is

[TABLE]

The parameters for the $\mathrm{GLC}$ method used in this example are summarized in Table 5.1.

Figure 5.11 summarizes the performance of the $\mathrm{GLC}$ algorithm on this problem.

5.5.6 Observations and Discussion

The first observation is that all algorithms tested generally produce lower cost solutions with increased run-time. In example 5.5.1, where the optimal cost was known a-priori, the $\mathrm{GLC}$ and $\mathrm{RRT}^{*}$ algorithms were observed to converge to this value, and the $\mathrm{SST}$ algorithm converged to some approximately optimal cost. In the remaining examples, where the optimal cost was unknown and $\mathrm{RRT}^{*}$ was not applicable, the $\mathrm{GLC}$ and $\mathrm{SST}$ algorithms were observed to produce solutions of generally decreasing cost with increased run-time. The local planning subroutine for the $\mathrm{RRT}^{*}$ algorithm in example 5.5.1 was simply the line segment connecting two points. This is requires virtually no computation to generate making $\mathrm{RRT}^{*}$ the preferred algorithm.

Another observation was that the cost of solutions output by the $\mathrm{GLC}$ algorithm generally decreased with increasing resolution, but did not decrease monotonically. This is not inconsistent with Corollary 5.4.1 since the result only claims convergence. The cause of the nonmonotonic convergence is that each run of Algorithm 1 operates on $\mathcal{U}_{R}$ for a fixed $R$, and since $\mathcal{U}_{R}\not\subset\mathcal{U}_{R+1}$ it is possible that an optimal signal in $\mathcal{U}_{R}$ may be better than any signal in $\mathcal{U}_{R+1}$ for certain values of $R$. In example 5.5.4, $\Omega_{R}$ was randomly generated which required averaging several trials. This had a smoothing effect on the performance curves which is analogous to the effect of averaging the outputs of $\mathrm{RRT}^{*}$ and $\mathrm{SST}$.

A very important distinction between the $\mathrm{GLC}$ and the other methods $\mathrm{RRT}^{*}$ and $\mathrm{SST}$ is that the $\mathrm{GLC}$ algorithm must run to completion before a solution is returned, while $\mathrm{RRT}^{*}$ and $\mathrm{SST}$ are incremental algorithms that can be interrupted at any time and return the current best solution. This is a desirable feature for real-time planning and will be an important next step in the development of this approach. While, $\mathrm{SST}$ offers similar theoretical guarantees to the $\mathrm{GLC}$ algorithm, the difference in running time is several orders of magnitude in all of the trials making the $\mathrm{GLC}$ preferable under most circumstances.

A final remark is that the $\mathrm{GLC}$ algorithm suffers from the curse of dimensionality like all other motion planning algorithms. The PSPACE-hardness of these problems suggests this issue will never be resolved. The examples demonstrate the $\mathrm{GLC}$ algorithm on state spaces of dimension two to six, and we observe that with increasing dimension, the time required to obtain visually acceptable trajectories increases rapidly with dimension. Example 5.5.4 had six states and required one to five seconds to produce visually acceptable trajectories. This suggests that six states is roughly the limit for real-time applications.

Chapter 6 Admissible Heuristics for Optimal Kinodynamic Motion Planning

Many graph search problems arising in robotics and artificial intelligence that would otherwise be intractable can be solved efficiently with an effective heuristic informing the search. However, efficiently obtaining a shortest path on a graph requires the heuristic to be admissible as described in the seminal paper introducing the $\rm A^{*}$ algorithm [hart1968formal]. In short, an admissible heuristic provides an estimate of the optimal cost to reach the goal from every vertex, but never overestimates the optimal cost. A good heuristic is one which closely underestimates the optimal cost-to-go from every vertex to the goal.

The workhorse heuristic in shortest path problems is the Euclidean distance from a given state to the goal. This heuristic is admissible irrespective of the obstacles in the environment since, in the complete absence of obstacles, the length of the shortest path from a particular state to the goal is the Euclidean distance between the two points. Returning to example 5.5.1 of the last chapter, using the Euclidean distance as a heuristic in the $\mathrm{GLC}$ algorithm reduces the number of iterations required by $84\%$ in comparison to a uniform cost search. Figure 6.1 shows a side-by-side comparison of the region of $X_{\mathrm{free}}$ explored by the informed search versus the uniform cost search.

The value of admissible heuristics for kinodynamic planning has already been identified in a number of works where admissible heuristics for individual problems have been derived and used to reduce the running time of planning algorithms [informedRRT, batchInformedRRT, hauser2015asymptotically, paden2016generalized]. The $\mathrm{GLC}$ method is among these.

While admissibility of a heuristic is an important concept it gives rise to two challenging questions in the context of kinodynamic motion planning. First, without a priori knowledge of the optimal cost-to-go, how do we verify the admissibility of a candidate heuristic; and second, how do we systematically construct good heuristics for kinodynamic motion planning problems? The goal of this chapter is to present a principled study of admissible heuristics for kinodynamic motion planning which answers these questions.

The Value Function:

The optimal cost-to-go or value function $V:X_{\mathrm{free}}\rightarrow\mathbb{R}$ is central to this discussion and describes the greatest lower bound on the cost to reach the goal set from the initial state $x_{0}\in X_{\mathrm{free}}$. That is,

[TABLE]

where $\mathcal{U}_{\mathrm{goal}}$ and $\mathcal{X}_{\mathrm{goal}}$ are redefined (with some abuse of notation) with respect to an initial condition $x_{0}$. It is important to note that this definition of $V$ implies that $V$ is well defined and unique. The following properties of $V$ follow immediately from the assumption $g(z,w)\geq 0$ in (2.2.4):

[TABLE]

It is well known in optimal control theory that when the value function is differentiable111The equivalence holds even when the value function is not differentiable if a generalized solution concept known as a viscosity solution is used [crandall1983viscosity]., obtaining $V(x_{0})$ in (6.0.1) for every $x_{0}$ in $X_{\mathrm{free}}$ is equivalent to solving the celebrated Hamilton-Jacobi-Bellman equation,

[TABLE]

with the boundary condition $V(z)=0$ for all $z$ in the closure of $X_{\mathrm{goal}}$. In general solving this equation is equivalent to solving Problem 2.2.1 from every initial condition in $X_{\mathrm{free}}$ which is considerably more difficult.

6.1 Graph-Based Approximations

Most kinodynamic motion planning algorithms, the $\mathrm{GLC}$ algorithm included, structure an approximation of the problem as a graph $(\mathcal{V},E)$ with paths in the graph corresponding to trajectories satisfying the dynamic model (2.1.3). Conceptually, shortest paths on the graph are in some sense faithful approximations of optimal feasible trajectories for the problem. This was analyzed for the $\mathrm{GLC}$ algorithm in Section 4.1.

The non-negativity of the cost function (2.2.4) enables a nonnegative edge-weight to be assigned to each edge corresponding to the cost of the trajectory in relation with that edge. The approximated problem can then be addressed using shortest path algorithms for graphs.

The value function $\hat{V}:\mathcal{V}\rightarrow\mathbb{R}$ on the weighted graph is analogous to the value function $V$ in the original problem. For a vertex $z$ in the graph, $\hat{V}(z)$ is the cost of a shortest path to one of the goal vertices: $\mathcal{V}\cap X_{\mathrm{goal}}$. Since the feasible trajectories represented by the graph are a subset of the feasible trajectories of the problem we have the inequality

[TABLE]

6.1.1 Admissible Heuristics

Recall that the $\mathrm{GLC}$ algorithm utilized a priority queue which evaluated candidate paths in a graph in order of their cost in (5.3.1). Evaluating candidates in some order of merit is known as a best first search. This is accomplished with an operation which is traditionally called "$\mathtt{pop}$" in software libraries222This operation is technically not a function in the usual sense. Rather, it is a relation.. When we have a heuristic $H:X_{\mathrm{free}}\rightarrow\mathbb{R}$ that estimates the remaining cost to reach the goal, the ordering of the priority queue is modified so that the $\mathtt{pop}$ operation returns an element of the queue $Q$ satisfying,

[TABLE]

This orders the priority queue according to the cost of the input signal $u$ plus the estimate of the remaining cost to reach the goal from the terminal state of the trajectory associated to the input signal $[\varphi_{x_{ic}}(u)](\tau(u)))$.

A heuristic $H$ for a problem with value function $V$ is admissible if it satisfies

[TABLE]

When a heuristic is admissible, the first signal found to be in $\mathcal{U}_{\mathrm{goal}}$ is an optimal signal within the approximation $\mathcal{U}_{R}$. In light of (6.1.1), an admissible heuristic for the kinodynamic motion planning problem will also be admissible for any graph-based approximation to the problem. For the remainder of this chapter, the set of candidate heuristics will be restricted to differentiable scalar valued functions on ${X}_{\rm free}$.

Another important concept related to heuristics is referred to as consistency333This usage of the term consistency is distinct from the usage in chapter 4. This is an unfortunate double usage of the term in robotics.. Consistency of a heuristic is similar to the triangle inequality for metric spaces (cf. Appendix B.1). To define consistency, the value function for the kinodynamic motion planning problem must be parametrized by the goal set. This is denoted $V(z;X_{\mathrm{goal}})$. A heuristic $H$ is consistent if,

[TABLE]

Note that the inequality above involves the optimal cost-to-go from $z$ to $y$. While a best first search with an admissible heuristic will return an optimal solution, if the heuristic is not consistent, then the search may not benefit from using the heuristic in terms of iterations required to terminate the search.

Since the value function is unknown it is difficult to check that (6.1.3) and (6.1.4) are satisfied for a particular heuristic $H$. The next result characterizes a convex subset of admissible heuristics. This sufficient condition for admissibility can be checked directly from the problem data.

Theorem 6.1.1**.**

A heuristic $H$ is admissible if the following two conditions are satisfied:

[TABLE]

[TABLE]

Further, the heuristic is consistent if

[TABLE]

Proof.

(admissibility) Let $u$ be an input signal resulting in a feasible trajectory $x$ terminating in the goal. By construction the terminal state $x(\tau(u))$ is in the goal set $X_{\mathrm{goal}}$. Therefore, $H(x(\tau(u)))\leq 0$ by (AH1). This implies the inequality

[TABLE]

Then by the fundamental theorem of calculus

[TABLE]

Applying the chain rule to the integrand yields

[TABLE]

Since $x$ is feasible, $x(t)\in X_{\mathrm{free}}$ for all $t\in[0,\tau(u)]$. Directly applying (AH2),

[TABLE]

The right hand side is now simply the cost $J_{x_{ic}}(u)$ associated to the input signal $u$.

[TABLE]

Since the choice of $u$ was arbitrary within $\mathcal{U}_{\mathrm{goal}}$, we conclude that $H(x(0))\leq J_{x_{ic}}(u)$ for every $u\in\mathcal{U}_{\mathrm{goal}}$ with associated trajectory $x$. Equivalently,

[TABLE]

(consistency) Let $u$ be an input signal resulting in a trajectory $x$ from $x(0)=z$ and terminating at $x(\tau(u))=y$. By a similar line of reasoning as for the admissibility argument observe that

[TABLE]

Thus, $H(z)-H(y)$ lower bounds $J_{x_{0}}(u)$ for any input signal resulting in a trajectory from $z$ to $y$. Since $V(\,\cdot\,;\{y\})$ is the greatest lower bound to the cost of such trajectories we have

[TABLE]

Rearranging the expression above yields the definition of consistency for $H(\,\cdot\,;X_{\mathrm{goal}})$. ∎

It is interesting to note that the admissibility (and consistency) conditions are affine constraints which implies that the subset of admissible heuristics satisfying (AH1) and (AH2) are convex.

Corollary 6.1.1**.**

The set of candidate heuristics satisfying the admissibility conditions (AH1) and (AH2) is convex.

Proof.

Choose $\lambda\in[0,1]$, and heuristics $H_{1}$ and $H_{2}$ satisfying (AH1) and (AH2). The convex combination is given by $\lambda H_{1}+(1-\lambda)H_{2}$. By (AH1), both $H_{1}(z)$ and $H_{2}(z)$ are less than or equal to zero for all $z$ in $X_{\mathrm{goal}}$. Thus,

[TABLE]

Then inserting the convex combination into (AH2) yields

[TABLE]

Since $H_{1}$ and $H_{2}$ individually satisfy (AH2), $\lambda\geq 0$, and $1-\lambda\geq 0$, the convex combination of $H_{1}$ and $H_{2}$ also satisfies (AH2). ∎

It is interesting that the set of admissible heuristics is convex, but the fact that this characterization is only a subset of all possible heuristics raises the question of whether there are any good heuristics (close to the value function $V$) contained in this subset. The next section discusses the usefulness of this set showing that it contains the value function.

6.2 Selecting an Admissible Heuristic

Considering that a good heuristic is one which closely underestimates the value function and that the admissibility conditions of Theorem 6.1.1 characterize a set of such underestimates, a natural approach to the selection of a heuristic would be to find an admissible heuristic whose integral on $X_{\mathrm{free}}$ is maximized444Note that the measure $m$ must be finite on $X_{\mathrm{free}}$.. Since the constraints are affine and this would be a linear functional of $H$, the heuristic selection is then a (infinite-dimensional) linear program:

[TABLE]

To justify this formulation, the next result shows that it is equivalent to the HJB equation and the solution to this linear program is in fact the value function.

Theorem 6.2.1**.**

A function $V$ is the solution to the linear optimization LP if and only if it is the solution to the Hamilton-Jacobi-Bellman equation HJB.

Proof.

($\Leftarrow$) Let $V$ be a solution to (HJB). Then $V$ is equal to the value function so every admissible heuristic $H$ must satisfy $H(z)\leq V(z)$ on $X_{\mathrm{free}}$. Therefore, the solution to (HJB) upper bounds the objective to (LP),

[TABLE]

Next, observe that a solution to (HJB) satisfies the admissibility conditions (AH1) and (AH2): The boundary condition $V(z)=0$ on $cl(X_{\mathrm{goal}})$ implies $V$ satisfies (AH2) and the equation

[TABLE]

implies

[TABLE]

which is (AH2). Thus, the upper bound in (6.2.1) is attained at the solution to (HJB).

($\Rightarrow$) As a point of contradiction, suppose that $H$ is a solution to (LP) and is not a solution to (HJB). Then there exists $\tilde{z}$ such that

[TABLE]

The existence of a point where there is strict inequality comes from the assumption that $H$ does not solve (HJB), but satisfies the constraints of (LP). By the continuity of $H$, there is a neighborhood $\mathcal{N}_{\tilde{z}}$ of $\tilde{z}$ on which (6.2.4) holds. Let $K$ be a compact subset of $\mathcal{N}_{\tilde{z}}$ and denote the minimum of $H$ attained on this compact set by $\varepsilon$. Additonally, denote the supremum of $\|f(z,w)\|_{2}$ over $K\times\Omega$ by $F$. Then

[TABLE]

Next, let $\Psi:X_{\mathrm{free}}\rightarrow\mathbb{R}^{n}$ be a smooth, nonnegative bump function supported on $K$ satisfying $\Psi(x)\leq\varepsilon$, and $\|\nabla\Psi(x)\|_{2}\leq\min\{1,\varepsilon/F\}$. Perturbing $H$ by $\Psi$ yields

[TABLE]

Thus, the perturbation by $\Psi$ leaves the heuristic admissible by (6.2.5) and this perturbation is a strict improvement to the objective in (LP) a contradiction since $H$ solves (LP). ∎

Thus, (HJB) and (LP) can both be solved to obtain the exact optimal cost-to-go for a problem. This equivalence between (HJB) and (LP) is a stronger version of the result than can be found in the published work [paden2017verification]. While the HJB equation is a nonlinear partial differential equation, the problem (LP) is a linear program lending itself to the methods of convex analysis.

6.2.1 Problem Relaxations

In many applications the set $X_{\mathrm{free}}$ is not entirely known a priori. This is particularly true when the heuristic is computed off-line and used in a real-time application where a perception system constructs or modifies $X_{\mathrm{free}}$ for the current task (e.g. detecting obstacles in a robot workspace).

This consideration motivates the following observation: suppose $X_{\mathrm{free}}$, $X_{\mathrm{goal}}$, $\Omega$, $f$, and $g$ is problem data for problem $P$; and $\tilde{X}_{\rm free}$, $\tilde{X}_{\rm goal}$, $\tilde{\Omega}$, $\tilde{g}$, and $\tilde{f}$ is problem data for problem $\tilde{P}$. Let $V_{P}$ and $V_{\tilde{P}}$ denote the optimal cost-to-go for each of these problems.

If the two problems are related by

[TABLE]

then any feasible trajectory of problem $P$ must also be feasible for problem $\tilde{P}$. Additionally, this trajectory will have the same or lesser cost for $\tilde{P}$. Therefore, $V_{\tilde{P}}(z)\leq V_{P}(z)$ for any $z\in\mathcal{X}_{\mathrm{free}}$. We can conclude that an admissible heuristic $H_{\tilde{P}}$ for problem $\tilde{P}$ must also be admissible for problem $P$ since $H_{\tilde{P}}(z)\leq V_{\tilde{P}}(z)$ implies $H_{\tilde{P}}(z)\leq V_{P}(z)$. Problem $\tilde{P}$ is referred to as a relaxation of problem $P$.

In the case of an unknown environment, one can derive an admissible heuristic for a relaxed problem which considers only constraints known in advance. This heuristic remains admissible if $X_{\mathrm{free}}$ is updated to a smaller set due to perceived obstacles. Alternatively, it may be easier to verify the admissibility of a candidate heuristic with (AH1) and (AH2), or evaluate (LP) on a relaxation of a particular problem known a-priori. This comes at the expense of increasing the gap between the heuristic and the optimal cost-to-go for the actual problem which can make the heuristic less effective for a search-based algorithm but allows for off-line construction.

6.3 Sum-of-Squares (SOS) Approximation to

Equation (LP)

One way to tackle (LP) in the case of problem data consisting of semi-algebraic sets and polynomials is a SOS programming approximation.

SOS programming [parrilo2004sum] is a method of optimizing a functional of a polynomial subject to semi-algebraic constraints. The technique involves replacing semi-algebraic constraints with sum-of-squares constraints which can then be solved as a semi-definite program (SDP). The advantage of this particular approach to approximating (LP) is that the result of the SOS program is guaranteed to satisfy the admissibility constraint.

6.3.1 Sum-of-Squares Polynomials

A polynomial $p$ in the ring $\mathbb{R}[z]$ in $n$ variables is said to be a sum-of-squares if it can be written as

[TABLE]

for polynomials $q_{k}(z)$. Clearly, $p(z)\geq 0$ for all $z\in\mathbb{R}^{n}$. Note also that $p(z)$ is a sum-of-squares if and only if it can be written as

[TABLE]

for a positive semi-definite matrix $Q$ and the vector of monomials $\mathfrak{m}(z)$ up to degree $d$. For a polynomial $p$ admitting a decomposition of the form (6.3.2) we write $p\in SOS$.

Equation (6.3.2) is a collection of linear equality constraints between the entries of $Q$ and the coefficients of $p(z)$. Finding entries of a positive semi-definite $Q$ such that the equality constraints are satisfied is then a semi-definite program (SDP). The complexity of finding a solution to this problem using interior-point methods is polynomial in the size of $Q$.

This method of analyzing polynomial inequalities has had a profound impact in many fields. As a result there are a number of efficient solvers [sedumi, sdpt3] and modeling tools [sostools, yalmip] available.

6.3.2 Optimizing the Heuristic

To proceed with computing a heuristic using the SOS programming framework, the problem data must consist of polynomials and intersections of semi-algebraic sets. Let

[TABLE]

for vectors of polynomials $h_{z}$ and $h_{w}$ with "$\geq$" denoting element-wise inequalities. Assume also that $f$, $g$ and the candidate heuristic $H$ are polynomials in the state and control variables. Then the admissibility condition (AH1) is a polynomial inequality. To restrict the non-negativity constraint of the heuristic to $X_{\rm free}$ and $\Omega$, we add auxiliary vectors of SOS polynomials $\lambda_{z}(z)\geq 0$ and $\lambda_{w}(w)\geq 0$ to the equation as

[TABLE]

which trivially implies the positivity of (AH1) over $X_{\mathrm{free}}$ and $\Omega$. When $H$ is a polynomial, the objective in (LP) is linear in the coefficients of $H$. Thus, it is an appropriate objective for an SOS program.

The SOS program which is solved to obtain an admissible heuristic is then

[TABLE]

6.4 Examples of Heuristics for Kinodynamic Motion Planning

The examples of this section demonstrate how to directly apply Theorem 6.1.1 to verify admissibility of candidate heuristics as well as the heuristic synthesis optimization (LP).

6.4.1 Admissibility of Euclidean Distance in Shortest Path Problems

In the first example we show how to use Theorem 6.1.1 to verify the classic Euclidean distance heuristic for kinematic shortest path problems.

Consider a holonomic shortest path problem,

[TABLE]

where $X_{\mathrm{free}}=\mathbb{R}^{n}$, $X_{\mathrm{goal}}=\{0\}$, and $u(t)\in\{w\in\mathbb{R}^{n}:\,\|w\|=1\}$. The running cost is given by

[TABLE]

We would like to verify the heuristic

[TABLE]

Applying the admissibility Lemma we obtain

[TABLE]

which reverifies the fact that the Euclidean distance is an admissible heuristic for the shortest path problem. The crux of this derivation is simply applying the Cauchy-Schwarz inequality. Figure 6.1 illustrates the use of this heuristic in an environment with obstacles.

6.4.2 Wheeled Robot Example

Consider a simple wheeled robot with state coordinates in $\mathbb{R}^{3}$, and whose mobility is described by

[TABLE]

where $(p_{1}(t),p_{2}(t),\theta(t))$ are individual coordinates of the state. Let $X_{\mathrm{free}}=\mathbb{R}^{3}$, $X_{\mathrm{goal}}=\{(0,0,0)\}$, and $\Omega=[-1,1]$. The running cost

[TABLE]

results in a cost equal to the path length in the $x$-$y$ plane.

We will consider two candidate heuristics: the line segment connecting the $p_{1}$-$p_{2}$ coordinate to the origin (discussed in [dolgov2010path]), and the magnitude of the heading error,

[TABLE]

Clearly, (AH1) is satisfied for both heuristics. Then (AH2) is once again verified with the Cauchy-Schwarz inequality.

For brevity, but with some abuse of notation, the time argument is dropped from the trajectory in the following derivations. Inserting the expression for the heuristics into (AH2) yields

[TABLE]

Similarly, for $H_{2}$,

[TABLE]

Therefore, both heuristics are admissible. Note that the maximum of the two heuristics is also an admissible heuristic,

[TABLE]

Figure 6.2 illustrates a shortest path query in a simple environment with and without the use of the heuristic in (6.4.10). The heuristically guided search obtains a solution in 81,686 iterations while the uniform cost search requires 403,197 iterations. In contrast, using just $H_{1}$ in Equation (6.4.7) as a heuristic requires 104,492 iterations.

6.4.3 Autonomous Underwater Vehicle Example

Consider an autonomous underwater vehicle (AUV) navigating a strong current relative to the vehicle’s top speed as discussed in [garau2014path]. This scenario is common for long range underwater gliders which travel at roughly $0.5\,\nicefrac{{m}}{{s}}$ relative to currents traveling at $1.0$-$1.5\,\nicefrac{{m}}{{s}}$.

Let the state space be $\mathbb{R}^{n}$ ($n=2$ or $3$) representing position in a local Cartesian coordinate system. The AUV’s motion is modeled by

[TABLE]

where $c:\mathbb{R}^{n}\rightarrow\mathbb{R}^{n}$ describes the current velocity at each point in the state space. The input $u(t)$ is the controlled velocity relative to the current. The thrust constraint is represented by

[TABLE]

where $w_{\max}$ is the maximum achievable speed relative to the current. The goal set is $X_{\mathrm{goal}}=\{0\}$. The running cost

[TABLE]

reflects a penalty on the duration of the path as well as the total work done by AUV’s motors.

The admissibility of the heuristic proposed in [garau2005path] can be evaluated using Theorem 6.1.1 . Let $v_{\max}\coloneqq\underset{z}{\max}\{\|c(z)\|\}$ be the maximum speed of the current, and consider the heuristic

[TABLE]

Clearly (AH1) is satisfied. Now evaluate (AH2),

[TABLE]

Notice that $v_{\max}$ and $w_{\max}$ were defined so that $\|w\|+\|c(z)\|\leq v_{\max}+w_{\max}$. Thus,

[TABLE]

so the heuristic is admissible. Figure 6.3 illustrates a numerical example in a 2D environment where the maximum current speed is 2.6 times the AUV’s maximum relative speed. The heuristically guided search expands $49\%$ fewer vertices than the uniform cost search.

6.5 Sum-of-Square Heuristic Synthesis Examples

The last examples demonstrate the SOS programming formulation described in Section 6.3. A closed form solution for the optimal cost-to-go is known for the examples in this section which provides a useful point of comparison for the computed heuristic.

The example problem was implemented using the SOS module in YALMIP [yalmip] and solved using SDPT3 for the underlying semi-definite program [sdpt3]. To further illustrate the approach, YALMIP scripts for these examples can be found in [SOS_heuristics].

6.5.1 Single Integrator Example

To illustrate the procedure, we revisit Example 6.4.1 in the 1-dimensional case. The differential constraint is given by $f(x,u)=u$ where $x,u\in\mathbb{R},$ $X_{\mathrm{free}}=[-1,1],$ $\Omega=[-1,1]$, and $X_{\mathrm{goal}}=\{0\}$. Again we use the minimum time objective where $g(x,u)=1$. The value function $V(x)=|x|$ is obtained by inspection.

The heuristic is parameterized by the coefficients of a univariate polynomial of degree $2d$

[TABLE]

Using a discrete measure on $[-1,1]$ concentrated at the boundary the SOS program is,

[TABLE]

The numerical solution for polynomial heuristics with increasing degree is shown in Figure 6.4.

6.5.2 Double Integrator Example

Consider a double integrator model,

[TABLE]

with the minimum time running cost

[TABLE]

free space $X_{\mathrm{free}}=[-3,3]^{2}$, input space $\Omega=[-1,1]$, and goal set $X_{\mathrm{goal}}=\{0\}$.

Polynomial heuristics of degree $2d$ of the form

[TABLE]

are computed with the measure $m$ in (LP) supported on $[-2,2]\times[-\sqrt{2},\sqrt{2}]$. This focuses the optimization in a region around the goal while maintaining admissibility of the heuristic over all of $X_{\mathrm{free}}$.

The SOS approximation of (LP) is formulated as follows

[TABLE]

Note that the polynomial $H$ can be integrated over the rectangular region $S$ in closed form with standard integration rules or with the integration functionality in YALMIP.

The optimized heuristics of increasing degree are shown in Figure 6.5 together with the value function for $X_{\mathrm{free}}=\mathbb{R}^{n}$. The percent error in Figure 6.5 is defined as

[TABLE]

6.5.3 Observations

The optimization of Example 6.5.2 focused on the region $[-2,2]\times[-\sqrt{2},\sqrt{2}]$ instead of $[-3,3]^{2}$. The reason is that some states in $[-3,3]^{2}$ cannot be reached and have infinite cost. A remarkable observation is that the resulting SOS program does not admit a maximum when the integral includes a subset of $X_{\mathrm{free}}$ where the value function is unbounded. This is consistent with the theoretical results since the heuristic is free to go unbounded over this set as well. Therefore, it is important to optimize the heuristic over a region where the optimal cost-to-go is known to be bounded.

While semi-definite programs run in polynomial time in problem size, there is combinatorial growth in the number of monomials with increasing degree resulting in large semi-definite programs. For an $n$-dimensional state space, the number of monomials of degree $d$ or less is

[TABLE]

This rapid growth in problem size is one of the current limitations of this approach to (LP). However, the technique produces a reasonable polynomial approximation that is guaranteed to be admissible.

There are a number possibilities for improving this technique. The DSOS and SDSOS [ahmadi2014dsos] programming techniques which are being developed will enable the derivation of heuristics with higher degree polynomials. Additionally, a straight forward solution is to apply finite difference methods to directly approximate (LP) as a finite-dimensional linear program, but the strict admissibility is lost in this case.

Chapter 7 Conclusion

This thesis presented a direct, forward search algorithm for approximate optimal kinodynamic motion planning called the generalized label correcting method. The contribution of this method to robotics is that it is resolution complete, does not rely on abstract subroutines which may be unavailable in practice, and the assumptions placed on problem data are readily verified.

In Chapter 3, techniques from topology and functional analysis were used to establish that the set of solutions to the problem is open and the performance objective is continuous. These properties were the foundation for the numerical approximation of the problem in Chapter 4, as well as the derivation of the generalized label correcting conditions in Chapter 5. The generalized label correcting method is a slight variation on canonical label correcting methods, making the method familiar and easily implemented. Numerical experiments tested the algorithm on a wide variety of motion planning problems demonstrating that, in addition to the theoretical guarantees, the method performs well in practice.

Like a best-first search, the generalized label correcting method can utilize an admissible heuristic to reduce the running time of the algorithm. This motivated the study of admissible heuristics for kinodynamic motion planning in Chapter 6, which resulted in tools for the verification and synthesis of admissible heuristics. The main result of Chapter 6 was an admissibility condition which is readily verified for a candidate heuristic and which characterizes a convex set of admissible heuristics. Additionally, the synthesis of an admissible heuristic was formulated as an infinite-dimensional linear program whose feasible set consisted of admissible heuristics and whose exact solution is equal to the optimal cost-to-go. Sum-of-squares programming was then proposed as a relaxation of this infinite-dimensional convex program which provides the best provably admissible heuristic within a finite-dimensional subspace of polynomials.

7.1 Extensions and Improvements

There are several directions of future research that will improve the utility of these methods. First and foremost, an anytime variation is needed to maximize effectiveness in real-time applications. Such a variation was omitted from this thesis to avoid obscuring the simplicity of the method. Secondly, the partitioning of the state space in Section 5.1 is admittedly simple and can be improved with additional information about the dynamical system. For example, the Ball-Box theorem from differential geometry can be leveraged to determine the most appropriate asymptotic scaling of the partition.

One limitation of the generalized label correcting algorithm as it was presented is that it was constructed under very weak assumptions. While this is an attractive property of the method, it leaves information in more structured problem instances unused. It is possible that, with added assumptions, sharper pruning conditions can be derived by similar arguments to those presented in Chapter 5.

Equipped with the understanding of the set of feasible trajectories, an exciting direction for future research is the application of this approach to more complex logical specifications. Simply adding specifications with boolean logic would greatly increase the expressiveness of the generalized label correcting method. However, more sophisticated modal logics, such as linear temporal logic, could be considered as well.

Appendix A Review of Set Theory

A set is a collection of logical variables which are called the elements of a set. A set is entirely determined by its elements. Shorthand for the sentence "$x$ is an element of $S$" is "$x\in S$".

A.1 Notation

Sets can be defined implicitly by the variables $x$ satisfying a logical formula $\mathcal{P}(x)$. Such a set is denoted $\{x:\,\mathcal{P}(x)\}$. Complex formulas can be constructed from atomic formulas using the logical connectives; "and ($\land$)", "or ($\vee$)", "not ($\neg$)", "implies ($\Leftarrow$)", and "equivalent to ($\Leftrightarrow$)"; and the quantifiers "there exists ($\exists$)", and "for all ($\forall$)".

Let $S$ and $U$ be sets. The set $S$ is said to be s subset of $U$, denoted $S\subset U$, if every element of $S$ is also an element of $U$:

[TABLE]

The set $S$ is equal to $U$ if $S$ is a subset of $U$ and $U$ is a subset of $S$:

[TABLE]

It is often useful to consider the set of all subsets of a set $S$. This set, denoted $2^{S}$, is called the power set of $S$,

[TABLE]

A set which contains no elements is called the empty set, denoted $\emptyset$.

A.2 Operations on Sets

The complement of $S$, denoted $S^{c}$, is the set whose elements are not elements of $S$:

[TABLE]

The union of $S$ and $U$, denoted $S\cup U$, is the set whose elements are an element of $S$ or $U$:

[TABLE]

The intersection between $S$ and $U$, denoted $S\cap U$, is the set whose elements are elements of $S$ and $U$:

[TABLE]

Two sets $S$ and $U$ are said to be disjoint if their intersection is empty, $S\cap U=\emptyset$. The exclusion of $U$ from $S$, denoted $S\setminus U$, is the set whose elements belong to $S$ and do not belong to $U$:

[TABLE]

There are a number of useful identities that follow directly from these operations. These are: associativity of intersection and union operations, $(A\cup B)\cup C=A\cup(B\cup C)$ and $(A\cap B)\cap C=A\cap(B\cap C)$; commutativity of intersection and union operations, $A\cup B=B\cup A$ and $A\cap B=B\cap A$; distributivity $A\cup(B\cap C)=(A\cup B)\cap(A\cup C)$ and $A\cap(B\cup C)=(A\cap B)\cup(A\cap C)$; DeMorgan’s law, $(A\cup B)^{c}=A^{c}\cap B^{c}$; and $A\setminus B=A\cap B^{c}$.

A cover of a set $S$ is a collection of sets whose union contains $S$ as a subset. That is, $\{U_{i}\}_{i\in\mathcal{I}}$ is a cover of $S$ if

[TABLE]

If $\{U_{i}\}_{i\in\mathcal{I}}$ is a cover of $S$, then a collection of subsets $\{U_{i}\}_{i\in\mathcal{J}}$ is a subcover of $\{U_{i}\}_{i\in\mathcal{I}}$ if $\mathcal{J}\subset\mathcal{I}$ and

[TABLE]

A cover $P$ of $S$ composed of pair-wise disjoint subsets is called a partition of $S$.

A.3 Binary Relations

Given two sets $U$ and $W$, a binary relation $R$ between $U$ and $W$ is a subset of $U\times W$ identifying the elements of $U$ related to elements of $W$. If $u\in U$ is related to $w\in W$, then $(u,w)\in R$ This is sometimes alternatively denoted $uRw$. As an example, suppose $S$ is a set of books. A binary relation $R$ on $S\times S$ could be ordered pairs of books where the first was published before the second. Then for $x,y\in S$, if $(x,y)\in R$, the statement "$x$ was published before $y$" is true.

Two important concepts are the inverse relation and composition relation. For any relation $R\subset U\times W$, the inverse relation is defined

[TABLE]

For two relations $R_{1}\subset V\times U$ and $R_{2}\subset U\times W$, the composition relation on $V\times W$ is defined

[TABLE]

A.3.1 Functions

A function $f$ from $X$ into $Y$, usually denoted $f:X\rightarrow Y$, is a binary relation in $X\times Y$ with the properties,

For all $x\in X$ there exists a $y\in Y$ such that $(x,y)\in f$. 2. 2.

If $(x,y_{1})\in f$ and $(x,y_{2})\in f$, then $y_{1}=y_{2}$.

In light of the uniqueness property, we write $f(x)$ to denote the element in $Y$ associated to $x$. The set $X$ is referred to as the domain of $f$ and $Y$ is referred to as the codomain.

The inverse relation $f^{-1}$ of a function $f$ is not necessarily a function. When the inverse relation of a function is itself a function, the original function is said to be invertible.

The following are a few useful properties relating how basic set operations are preserved between the domain and codomain of a function. Let $f$ be a function from $X$ into $Y$ with $A_{i}\subset X$ and $B_{i}\subset Y$.

$f(A_{1}\cup A_{2})=f(A_{1})\cup f(A_{2})$, 2. 2.

$f(A_{1}\cap A_{2})\subset f(A_{1})\cap f(A_{2})$, 3. 3.

$f(A_{1})\setminus f(A_{2})\subset f(A_{1}\setminus A_{2})$ 4. 4.

$f^{-1}(B_{1}\cup B_{2})=f^{-1}(B_{1})\cup f^{-1}(B_{2})$, 5. 5.

$f^{-1}(B_{1}\cap B_{2})=f^{-1}(B_{1})\cap f^{-1}(B_{2})$, 6. 6.

$f^{-1}(B_{2}\setminus B_{1})=f^{-1}(B_{2})\setminus f^{-1}(B_{1})$

For any set $S$, a sequence on $S$ is a function in $\mathbb{N}\times S$ or $\{1,2,...,n\}\times S$. While one would normally write $s(i)$ for the element $(i,s)$ in the sequence, the notation $\{s_{i}\}_{i\in\mathbb{N}}$ is usually used. When the index set is clear, this is abbreviated $\{s_{i}\}$. A subsequence of $s_{j}$ of $s_{i}$ is a function from a subset of the natural numbers into $S$ which is equal to $s_{i}$ when $i=j$.

If $f$ is a function from $X$ into $X$, then a fixed point of $f$ is an element $x\in X$ satisfying $(x,x)\in f$ (usually written $f(x)=x$).

A.3.2 Orders

An order on a set $X$ is a binary relation $R\subset X\times X$ with the properties,

$(x,x)\in R$. 2. 2.

If $(x,y)\in R$ and $(y,x)\in R$, then $x=y$. 3. 3.

If $(x,y)\in R$ and $(y,z)\in R$, then $(x,z)\in R$. 4. 4.

$(x,y)\in R$ or $(y,x)\in R$ for all $x,y\in X$.

A relation in $X\times X$ is a partial order if it only satisfies items 1-3. It is a strict partial order if it satisfies items 2-3 and the negation of item 1. The notation for an order $R\subset X\times X$ is usually denoted $x\leq y$ for $(x,y)\in R$.

Let $(X,\leq)$ be an ordered set. A lower bound $l$ on a subset $S$ of $X$ is an element of $X$ such that $l\leq s$ for all $s\in S$. Similarly, an upper bound $u$ on a subset $S$ of $X$ is an element of $X$ such that $s\leq u$ for all $s\in S$. Now let $L_{S}$ be the set of lower bounds of $S$, and $U_{S}$ be the set of upper bounds of $S$. If $L_{S}$ is nonempty, then $S$ is said to be bounded from below. Similarly, if $U_{S}$ is nonempty, then $S$ is said to be bounded from above. The ordered set $(X,\leq)$ is said to have the greatest lower bound property if, for every nonempty $S$ that is bounded from below, there exists a lower bound $l^{*}\in X$ of $S$ which is also an upper bound of $L_{S}$. We call $l^{*}$ a greatest lower bound or infimum. Similarly, the ordered set $(X,\leq)$ is said to have the least upper bound property if, for every nonempty $S$ that is bounded from above, there exists an upper bound $u^{*}\in X$ of $S$ which is also a lower bound of $U_{S}$. We call $u^{*}$ a least upper bound or supremum.

A direct consequence of these definitions is that an ordered set has the greatest lower bound property if and only if it has the least upper bound property. Additionally, if an ordered set admits an infimum/supremum it is unique.

If a subset of an ordered set contains its infimum/supremum, then we say that the set admits a minimum/maximum element.

A.3.3 Equivalence Relations

An equivalence relation on a set $X$ is a binary relation $R\subset X\times X$ with the properties,

$(x,x)\in R$ for all $x\in X$. 2. 2.

$(x,y)\in R$ if and only if $(y,x)\in R$. 3. 3.

If $(x,y)\in R$ and $(y,z)\in R$, then $(x,z)\in R$.

The notation for an equivalence relation is usually denoted $x\sim y$ if $(x,y)\in R$. The equivalence class of an element $z\in X$ is the set of all $x\in X$ such that $z\sim x$. This is denoted

[TABLE]

Clearly, the set of all equivalence classes is a cover of $X$ since

[TABLE]

Additionally, if $[x]$ and $[y]$ are equivalence classes, either $[x]=[y]$ or $[x]\cap[y]=\emptyset$. Thus, every equivalence relation induces a partition of $X$ composed of the equivalence classes of the relation. Conversely, every partition $P$ of $X$ induces an equivalence relation where $x\sim y$ if $x,y\in P$.

Appendix B Review of Mathematical Analysis

This appendix on is based on personal notes from the following sources [kelley1975general, munkres2000topology, rudin1964principles].

B.1 Topological Spaces

Equipping sets with a topology makes it possible to discuss continuity of functions, limits of sequences, compactness, and other useful properties.

A set $X$ together with a collection of subsets $\mathcal{T}_{X}$ is called a topological space on $X$ if

• TS1

$\emptyset\in\mathcal{T}_{X}$ and $X\in\mathcal{T}_{X}$.

• TS2

If $S_{i}\in\mathcal{T}_{X}$ for every $i\in I$, then $\bigcup_{i\in I}S_{i}\in\mathcal{T}_{X}$.

• TS3

$U\cap S\in\mathcal{T}_{X}$ for every $U,S\in\mathcal{T}_{X}$.

A collection of subsets of $X$ satisfying axioms TS1-TS3 is called a topology on $X$. The elements of a topology are called open sets. A set which is the complement of an open set is called a closed set. Note that a set can be both open and closed (e.g. $\emptyset$ in any topological space).

The interior of a subset $S$, denoted $int(S)$, is the union of open subsets of $S$. The closure of a subset $S$, denoted $cl(S)$ is the intersection of closed sets containing $S$. A neighborhood $\mathcal{N}_{p}$ of a point $p$ in $X$ is an element of $\mathcal{T}_{X}$ containing $p$.

A point $p$ in a subset $S$ of $X$ is said to be a limit point of $S$ if every neighborhood of $p$ has a nonempty intersection with $S\setminus\{p\}$. A sequence $\{x_{i}\}$ in $X$ is said to converge to a point $p$ in $X$ if, for any neighborhood $\mathcal{N}_{p}$ of $p$, there exists an $N\in\mathbb{N}$ such that $n>\mathbb{N}$ implies $x_{i}\in\mathcal{N}_{p}$.

If $S$ is a subset of $X$, then the subspace topology $\mathcal{T}_{S}$ on $S$ is defined by

[TABLE]

Lemma B.1.1**.**

If $S$ is an element of $\mathcal{T}_{X}$, then the subspace topology $\mathcal{T}_{S}$ is a subset of $\mathcal{T}_{X}$.

Proof.

If $S$ is an element of $\mathcal{T}_{X}$ then so is $S\cap U$ for any $U$ in $\mathcal{T}_{X}$ by TS3. ∎

It is important to note that if $S$ is not open, elements of the subspace topology defined by $S$ may not be elements of the original topology.

A collection of subsets $\mathcal{B}_{X}$ of $X$ is called a base for a topology if it is a cover of $X$, and for every $B_{i},B_{j}\in\mathcal{B}_{X}$, if $x$ is an element of $B_{i}\cap B_{j}$, then there exists a $B_{k}\subset B_{i}\cap B_{j}$ in $\mathcal{B}_{X}$ such that $x$ is an element of $B_{k}$. A topological space is second countable if it has a countable base.

Lemma B.1.2**.**

Let $\mathcal{B}_{X}$ be a base for a topology on $X$, and let $\mathcal{T}_{B}$ be the collection of subsets of $X$ satisfying: for every $U\in\mathcal{T}_{B}$ and $x\in U$, there exists $B\in\mathcal{B}_{X}$ such that $x\in B$ and $B\subset U$. The collection of subsets $\mathcal{T}_{B}$ is a topology on $X$ called the topology generated by $\mathcal{B}_{X}$.

Proof.

We need to show that $\mathcal{T}_{B}$ is a topology.

(TS1) It is vacuously true that the empty set belongs to $\mathcal{T}_{B}$. Next, since $\mathcal{B}_{X}$ is a cover of $X$, there exists a $B\in\mathcal{B}_{X}$ such that $x\in B$ for every $x\in X$ and clearly $B\subset X$. Thus, $X\in\mathcal{T}_{B}$.

(TS2) Let $U_{i}$ be an element of $\mathcal{T}_{B}$ for all $i\in\mathcal{I}$ and define

[TABLE]

For each $x\in U$, there is an $i\in\mathcal{I}$ such that $x\in U_{i}$. Now by construction of $\mathcal{T}_{B}$, there exists $B\in\mathcal{B}_{X}$ such that $x\in B$ and $B\subset U_{i}$. Then $x\in U$ and $B\subset U$ so that $U\in\mathcal{T}_{B}$.

(TS3) Consider $U_{1},U_{2}\in\mathcal{T}_{B}$ and $x\in U_{1}\cap U_{2}$. By construction of $\mathcal{T}_{B}$, there exists $B_{1},B_{2}\in\mathcal{B}_{X}$ such that $x\in B_{1}\subset U_{1}$, and $x\in B_{2}\subset U_{2}$. By definition of a base, there exists $B_{3}$ such that $B_{3}\subset B_{1}\cap B_{2}$ and $x\in B_{3}$. Thus, $B_{3}\subset U_{1}\cap U_{2}$ which, by construction of $\mathcal{T}_{B}$, implies $U_{1}\cap U_{2}\in\mathcal{T}_{B}$. ∎

B.1.1 Compactness

A subset of a topological space is said to be compact if every open cover has a finite subcover.

Lemma B.1.3**.**

If $S$ is a closed subset of a compact set $C$, then $S$ is compact.

Proof.

Let $\{U_{i}\}_{i\in\mathcal{I}}$ be an open cover of $S$. Then $\{S^{c}\}\cup\{U_{i}\}_{i\in\mathcal{I}}$ is an open cover of $C$ which will have a finite subcover $\{W_{i}\}_{i\in\mathcal{J}}$. Then $\{W_{i}\}_{i\in\mathcal{J}}\setminus\{S^{c}\}$ is a finite subcover of $\{U_{i}\}_{i\in\mathcal{I}}$. ∎

Lemma B.1.4**.**

If $C$ is a compact subset of a topological space, then every sequence $\{c_{n}\}$ contained in $C$ has a subsequence converging to a point in $C$.

Proof.

Suppose that the image of the sequence $\{c_{n}\}$ is finite. Then $c_{n}$ maps to at least one point $p$ in $C$ an infinite number of times in which case the subsequence whose image is $p$ converges to $p$. Now suppose $\{c_{n}\}$ is infinite, and as a point of contradiction that this subset has no limit points. Then it is closed which implies it is compact as well since a closed subset of a compact set is compact. Then every point in $\{c_{n}\}$ will have a neighborhood $\mathcal{N}_{c_{n}}$ containing only the point $c_{n}$. This open cover of $\{c_{n}\}$ cannot have a finite subcover so it is not compact which is a contradiction. Thus, $\{c_{n}\}$ must have a limit point in $C$ from which a subsequence converging to that limit point can be constructed. ∎

It is interesting to note that the converse is also true, but this fact is not required for the arguments in this thesis.

B.1.2 Continuity

A function from a topological space $(X,\mathcal{T}_{X})$ into a topological space $(Y,\mathcal{T}_{Y})$ is said to be continuous if the inverse image of every open subset of $Y$ under $f$ is an open subset of $X$. That is,

[TABLE]

Alternatively, the function $f$ is continuous on a subset $S$ of $X$, if $f$ satisfies the above definition of continuity in the subspace topology associated with $S$. That is,

[TABLE]

It is important to point out that is is not necessarily true that $f^{-1}(U)\cap S\in\mathcal{T}_{S}$ implies $f^{-1}(U)\cap S\in\mathcal{T}_{X}$ (i.e. the inverse image intersected with $S$ need not be open in $\mathcal{T}_{X}$).

Lemma B.1.5**.**

Let $f$ be a continuous function from a topological space $(X,\mathcal{T}_{X})$ into a topological space $(Y,\mathcal{T}_{Y})$. If $C$ is a compact subset of $X$, then $f(C)$ is a compact subset of $Y$.

Proof.

Let $\{U_{i}\}_{i\in\mathcal{I}}$ be an open cover of $f(C)$. By the definition of continuity and TS2, $\bigcup_{i\in\mathcal{I}}f^{-1}(U_{i})$ is an open cover of $C$. Since $C$ is compact this cover contains a finite subcover $\{f^{-1}(U_{i})\}_{i\in\mathcal{J}}$ of $C$. Then

[TABLE]

which is an open cover of $f(C)$. Therefore, $C$ is compact. ∎

B.2 Metric Spaces

Metric spaces are sets equipped with a distance function that assign a distance between two points in the set. This distance function is a generalization of distance as we encounter it in two or three dimensional space.

A set $X$ together with a function $d_{X}:X\times X\rightarrow[0,\infty)$ is called a metric space if

• MS1

$0\leq d_{X}(x,y)$ for every $x,y$ in $X$.

• MS2

$d_{X}(x,y)=0$ if and only if $x=y$.

• MS3

$d(x,y)=d(y,x)$ for every $x,y$ in $X$.

• MS4

$d(x,z)\leq d(x,y)+d(y,z)$ for every $x,y,z$ in $X$

A distance function satisfying axioms MS1-MS4 is called metric on $X$. Notice that if $(X,d_{X})$ is a metric space and $S\subset X$, then $(S,d_{X})$ is a metric space. This is referred to as a metric subspace.

The open ball of radius $r$ centered at $p$ in a metric space $(X,d_{X})$ is defined

[TABLE]

Lemma B.2.1**.**

Every metric space $(X,d_{X})$ has an induced topology generated by $\{B_{r}(x)\}_{x\in X,\,r>0}$. This topology is called the metric topology.

Proof.

It is sufficient to show that $\{B_{r}(x)\}_{x\in X,\,r>0}$ is a base for a topology. Clearly, $\{B_{r}(x)\}_{x\in X,\,r>0}$ is a cover of $X$ since $x\in B_{r}(x)$ for any $r>0$. Now let $x$ be an element of $B_{r_{1}}(p_{1})\cap B_{r_{2}}(p_{2})$. Denote $d_{X}(x,p_{1})$ by $d_{1}$ and $d_{X}(x,p_{2})$ by $d_{2}$. Then $B_{(r_{1}-d_{1})}(x)\subset B_{r_{1}}(p_{1})$ and $B_{(r_{2}-d_{2})}(x)\subset B_{r_{2}}(p_{2})$. Next, let $r_{3}=\min\{r_{1}-d_{2},r_{2}-d_{2}\}$. Then $B_{r_{3}}(x)\subset B_{r_{1}}(p_{1})\cap B_{r_{2}}(p_{2})$. Therefore, $\{B_{r}(x)\}_{x\in X,\,r>0}$ is a base for a topology. ∎

With the induced topology, every metric space inherits the analytical tools available for a topological space.

B.2.1 Continuity in Metric Spaces

Metric spaces gives rise to an alternative interpretation of continuity:

Lemma B.2.2**.**

Suppose $f$ is a function from a metric space $(X,d_{X})$ into $(Y,d_{Y})$. If for every $p\in X$ and $\varepsilon>0$ there exists a $\delta>0$ (depending on $p$ and $\varepsilon$) such that $d_{X}(p,x)<\delta$ implies $d_{Y}(f(p),f(x))<\varepsilon$, then $f$ is continuous in the metric topology.

Proof.

Let $U$ be an open set in $Y$ with respect to the metric topology. Choose any $p\in f^{-1}(U)$. It will be sufficient to show that $p$ is an interior point of $f^{-1}(U)$. Since $U$ is open, there exists $B_{\varepsilon}(f(p))$ which is a subset of $U$. Now suppose that for every $\varepsilon>0$ there exists a $\delta>0$ such that $d_{X}(p,x)<\delta$ implies $d_{Y}(f(p),f(x))<\varepsilon$. Then $f^{-1}(B_{\varepsilon}(f(p)))\subset B_{\delta}(p)$. Therefore, $B_{\delta}(p)\subset f^{-1}(U)$ and, $p$ is an interior point of $f^{-1}(U)$. Thus, $f^{-1}(U)$ is open for any open set $U$ which is the definition of continuity. ∎

The converse is also true making this an alternative definition of continuity.

One can define stronger forms of continuity in metric spaces. A function $f$ from a metric space $(X,d_{X})$ into a metric space $(Y,d_{Y})$ is said to be uniformly continuous if for every $\varepsilon>0$ there exists a $\delta>0$ such that $d_{X}(x_{1},x_{2})<\delta$ implies $d_{Y}(f(x_{1}),f(x_{2}))<\varepsilon$. In contrast to the basic form of continuity, the $\delta$ in this definition can be independent of the points $x_{1}$, $x_{2}$ and depends only on $\varepsilon$.

Lemma B.2.3**.**

A continuous function $f$ from a compact metric space $(X,d_{X})$ into a metric space $(Y,d_{Y})$ is uniformly continuous.

Proof.

Since $f$ is continuous, for every $\varepsilon>0$ and $x\in X$, there exists a ball $B_{\delta_{x}}(x)$ such that $f(B_{\delta_{x}}(x))\subset B_{\varepsilon}(f(x))$. For each $\varepsilon>0$, the balls $B_{\delta_{x}/2}(x)$ form an open cover of $X$. Thus, there is a finite subcover defined by a finite set of points $\{x_{1},...,x_{n}\}$. Now let $\delta^{*}=\min\{\delta_{x_{1}},...,\delta_{x_{n}}\}$. Since this set is finite, it admits a strictly positive minimum. Now choose $p_{1},p_{2}\in X$ such that $d_{X}(p_{1},p_{2})<\delta^{*}/2$. Then $d_{X}(p_{1},x_{m})<\delta^{*}/2$ for one of the points $x_{m}$ and by the triangle inequality $d_{X}(p_{2},x_{m})<2\delta^{*}$. Thus, $p_{1},p_{2}\in B_{\delta^{*}}(x_{m})$ which implies $d_{X}(f(p_{1}),f(p_{2}))<\varepsilon$. $\delta^{*}$ is independent of $p_{1}$ and $p_{2}$ and therefore, $f$ is uniformly continuous. ∎

An even stronger form of continuity is Lipschitz continuity. A function $f$ from a metric space $(X,d_{X})$ into a metric space $(Y,d_{Y})$ is said to be Lipschitz continuous if there exists a constant $L$ such that

[TABLE]

The function ${\rm Lip}$ from the set of functions between two metric spaces into $\mathbb{R}$ will be defined as the greatest lower bound on the set of Lipschitz constants of its argument. That is, if $f$ is a function between two metric spaces, then ${\rm Lip}(f)$ is the infimum over Lipschitz constants of $f$.

Lemma B.2.4**.**

If $C$ and $U$ are disjoint subsets of a metric space $(X,d_{X})$, with $C$ compact and $U$ closed, then there exists a $\rho>0$ such that

[TABLE]

Proof.

As a point of contradiction suppose that the claim is false. Then there exists a sequence $\{c_{n}\}$ contained in $C$ and a point $u$ in $U$ such that $\lim_{n\rightarrow\infty}d(c_{n},u)=0$. Equivalently, $\lim_{n\rightarrow\infty}c_{n}=u$. Since $C$ is a compact set, there is a subsequence $\{c_{m}\}$ of $\{c_{n}\}$ which converges to a point $c$ in $C$. Then by the triangle inequality,

[TABLE]

Thus, $c=u$. Since $c$ is a point in $C$, $u$ is a point in $U$, and $C\cap U$ is the empty set, we arrive at a contradiction. ∎

Corollary B.2.1** (to Lemma B.2.4).**

If $C$ is a compact subset of an open set $S$ in a metric space $(X,d_{X})$, then there exists a $\rho>0$ such that

[TABLE]

Proof.

Since $C$ is a subset of $S$, it is disjoint with $S^{c}$, and since $S$ is open, $S^{c}$ is closed. Then by Lemma B.2.4 there is a $\rho>0$ such that $d_{X}(c,u)>\rho$ for all $c$ in $C$ and $u$ in $S^{c}$. Equivalently, $B_{\rho}(c)\cap S^{c}=\emptyset$ which implies $B_{\rho}(c)\subset S.$ ∎

B.2.2 Complete Metric Spaces

A sequence $x_{n}$ in a metric space $(X,d_{X})$ is said to be a Cauchy sequence if for every $\varepsilon>0$, there exists an $N$ such that $n,m>N$ implies $d_{X}(x_{n},x_{m})<\varepsilon$. The metric space $(X,d_{X})$ is said to be a complete metric space if every Cauchy sequence converges to a point in $X$.

Lemma B.2.5**.**

If $C$ is a closed subset of a complete metric space $(X,d_{X})$, then $(C,d_{X})$ is a complete metric space.

Proof.

Let $x_{n}$ be a Cauchy sequence in $C$. Then since $X$ is complete, $\lim_{n\rightarrow\infty}x_{n}=x$ for some $x\in X$. However, $x$ is then a limit point of $C$ and since $C$ is closed, $x\in C$. Thus, $(C,d_{X})$ is complete. ∎

A function $f$ from a metric space $(X,d_{X})$ into $(X,d_{X})$ is called a contraction if there exists $\gamma\in[0,1)$ such that

[TABLE]

Theorem B.2.1** (Banach fixed point theorem).**

If $f$ is a contraction on a complete metric space $(X,d_{X})$, then there exists a unique fixed point of $f$ in $X$.

Proof.

Let $x_{0}$ be any point in $X$ and define the sequence $x_{n}$ recursively as

[TABLE]

Then since $f$ is a contraction, there exists $\gamma<1$ such that

[TABLE]

which by induction yields

[TABLE]

Next we show that $x_{n}$ is a Cauchy sequence. Choose $m>n$ and apply the triangle inequality MS3 to $d_{X}(x_{n},x_{m})$,

[TABLE]

Now apply equation B.2.9 to each term in the right hand side of above expression,

[TABLE]

Now using the formula for geometric series we obtain

[TABLE]

which can be made arbitrarily small by choosing $n$ large enough. Thus, $x_{n}$ is a Cauchy sequence so it must converge to $x^{*}\in X$ by assumption that $(X,d_{X})$ is complete. Then

[TABLE]

which is equivalent to

[TABLE]

Now note that since $f$ is a contraction, it is Lipschitz continuous and therefore

[TABLE]

Thus, $f(x^{*})=x^{*}$ so $x^{*}$ is a fixed point of $f$. To prove uniqueness, suppose that $x^{*}$ and $y^{*}$ are fixed points of $f$. Then $d_{x}(f(x^{*}),f(y^{*}))\leq\gamma d_{X}(x^{*},y^{*})$ and $d_{x}(f(x^{*}),f(y^{*}))=d_{x}(x^{*},y^{*})$. The only solution to both the equation and inequality is if $d_{X}(x^{*},y^{*})=0$ in which case $x^{*}=y^{*}$ by MS1. ∎

B.3 Normed Vector Spaces

A vector space is an algebraic structure consisting of a set of vectors and a field111throughout this thesis the only field considered is $\mathbb{R}$. of scalars. A vector space is equipped with an associative and commutative addition "+" operation between vectors and a multiplication "$\cdot$" operation between a scalar and a vector that is distributive over vectors. In addition, there is an additive identity vector, denoted "[math]", such that $v+0=v$ for every vector $v$. Lastly, there is an additive inverse to every vector $v$, denoted $-v$ such that $v+(-v)=0$. This is abbreviated with $v-v$.

A normed vector space is a vector space equipped with a function $\|\cdot\|$ mapping vectors into $[0,\infty)$. This function, called a norm must satisfy

• NS1

$\|0\|=0$,

• NS2

$\|v\|>0\qquad\forall v\in\mathcal{V}\setminus\{0\}$,

• NS3

$\|\alpha v\|=|\alpha|\|v\|\qquad\forall\alpha\in\mathbb{R},v\in\mathcal{V}$,

• NS4

$\|v+w\|\leq\|v\|+\|w\|\qquad\forall v,w\in\mathcal{V}$.

An example of a norm on the vector space $\mathbb{R}^{n}$ is the $L_{p}$ norm for $p=1,2,...,\infty$:

[TABLE]

Lemma B.3.1**.**

For every normed vector space $(\mathcal{V},\|\cdot\|)$, there is an induced metric space $(\mathcal{V},d_{\mathcal{V}})$ with the metric $d_{\mathcal{V}}$ defined by $d_{\mathcal{V}}(v,w)=\|v-w\|$.

Proof.

We just need to show that MS1-MS4 follow from the definition of the metric and NS1-NS4. Suppose $v=w$. Then $\|v-w\|$ is the same as writing $\|v-v\|$ which is equal to [math] by NS1. Thus, MS1 is satisfied. Suppose $v\neq w$ so that $v-w\neq 0$. Then $\|v-w\|>0$ by NS2. Thus, MS2 is satisfied. Symmetry of the metric follows from the compatibility of scalar and vector multiplication, $\|v-w\|=|-1|\|(-1)\cdot v-(-1)\cdot w\|=\|w-v\|$. Thus, MS3 is satisfied. Next, the triangle inequality for the metric is just a rearrangement of the triangle inequality for the norm NS4,

[TABLE]

Thus, MS4 is satisfied. ∎

B.3.1 Banach Spaces

A Banach space is a normed vector space $(\mathcal{V},\|\cdot\|)$ with the property that the induced metric space is complete. The $\mathbb{R}^{n}$ with any of the $L_{p}$ norms is a Banach space.

Now consider the set of bounded functions $\mathscr{B}$ mapping a compact normed space $(X,\|\cdot\|_{X})$ into a Banach space $(Y,\|\cdot\|_{Y})$. The following is a norm on $\mathscr{B}$,

[TABLE]

Lemma B.3.2**.**

The normed space $(\mathscr{B},\|\cdot\|_{\infty})$ is a Banach space.

Proof.

Let $f_{n}$ be a Cauchy sequence in $(\mathscr{B},\|\cdot\|_{\infty})$. Then since $\|f_{n}(x)-f_{m}(x)\|_{Y}\leq\|f_{n}-f_{m}\|_{\infty}$ we have that $f_{n}(x)$ is a Cauchy sequence in $(Y,\|\cdot\|_{Y})$ for every $x\in X$. Since $Y$ is complete, a point-wise limit of $f_{n}$ exists which we denote $f$. It remains to show that the limit $f$ is bounded. While not all Cauchy sequences converge (if the space is not complete) they are always bounded. There exists an $M$ such that $\|f_{n}\|<M$ for all $n$. Therefore, $\|f_{n}(x)\|_{Y}\leq\|f_{n}\|_{\infty}<M$ for all $n\in\mathbb{N}$ and $x\in X$. Thus, $\|f(x)\|_{Y}<M$ which implies $f$ is a bounded function. Thus, the Cauchy sequence converges to $f\in\mathscr{B}$. ∎

Now let $\mathscr{C}$ be the subset of continuous functions in $\mathscr{B}$.

Lemma B.3.3**.**

$\mathscr{C}$* is a closed subset of the Banach space $(\mathscr{B},\|\cdot\|_{\infty}).$*

Proof.

Let $f$ be a limit point of $\mathscr{C}$. Then there exists $f_{n}\in\mathscr{C}$ which converges to $f\in\mathscr{B}$. For each $\varepsilon>0$ there exists $N>0$ such that $\|f_{n}-f\|_{\infty}<\varepsilon$ for all $n>N$. Since $f_{n}$ is continuous on a compact set $X$, it is uniformly continuous by Lemma B.2.3. Thus, there exists $\delta>0$ such that $\|f_{n}(x_{1})-f_{n}(x_{2})\|_{Y}<\varepsilon$ for all $n>N$ and $x_{1},x_{2}\in X$ satisfying $\|x_{1}-x_{2}\|_{X}<\delta$.

Next, we use the triangle inequality to show that $f$ is continuous and is therefore contained in $\mathscr{C}$.

[TABLE]

Observe that $\|f(x_{1})-f_{n}(x_{1})\|_{Y}$ and $\|f_{n}(x_{2})-f(x_{2})\|_{Y}$ are each less than $\|f-f_{n}\|_{\infty}$ which is less than $\varepsilon$. Then since $f_{n}$ is uniformly continuous we were able to choose $x_{1},x_{2}$ sufficiently close together so that $\|f_{n}(x_{1})-f_{n}(x_{2})\|_{Y}$ was also less than $\varepsilon$. Thus,

[TABLE]

which implies that $f$ is continuous and is therefore contained in $\mathscr{C}$. Since $f$ was an arbitrary limit point of $\mathscr{C}$ we conclude that $\mathscr{C}$ is closed. ∎

Theorem B.3.1**.**

The normed space $(\mathscr{C},\|\cdot\|_{\infty})$ is a Banach space.

Proof.

The normed space $(\mathscr{B},\|\cdot\|_{\infty})$ is a Banach space by Lemma B.3.2 and by Lemma B.3.3, $\mathscr{C}$ is a closed subset of $\mathscr{B}$. If follows from Lemma B.2.5 that $(\mathscr{C},\|\cdot\|_{\infty})$ is a Banach space. ∎

Appendix C Analysis of Functions and Integration

The content of this appendix is based on personal notes compiled from the following sources [coddington1955theory, feldman1981proof, kolmogorov1961elements, salamon2016measure].

C.1 Measure Spaces

It is difficult to have a precise discussion about dynamical systems without Legesgue integration and measure spaces which utilize the concept of measure for sets. The assignment of measure to subsets of the measure space is the generalization of assigning weight to shapes with volume. While the concept is very intuitive, one subtlety with measure spaces is that not all subsets can be assigned measure in general.

A set $X$ together with a collection $\mathscr{X}$ of subsets, and a function $m:\mathcal{S}\rightarrow[0,\infty]$ is called a measure space if

• SA1

$\emptyset\in\mathcal{X}$, $X\in\mathscr{X}$.

• SA2

$X^{c}\in\mathcal{X}$ for every $X$ in $\mathscr{X}$.

• SA3

$\bigcup_{n\in\mathbb{N}}X_{n}\in\mathscr{X}$ and $\bigcap_{n\in\mathbb{N}}X_{n}$ for any sequence $X_{n}$ contained in $\mathscr{X}$.

• SA4

$m(\emptyset)=0$.

• SA5

$m\left(\bigcup_{n\in\mathbb{N}}X_{n}\right)=\sum_{n\in\mathbb{N}}m(X_{n})$ for all sequences $X_{n}$ in $\mathscr{X}$ such that $X_{i}\cap X_{j}=\emptyset$ when $i\neq j$.

The collection of subsets $\mathscr{X}$ is called a $\sigma$-algebra and the function $m$ is called a measure. A set $S$ is said to be measurable if $S$ is an element of $\mathscr{X}$. We say that a proposition $p(x)$ is true almost everywhere in the measure space $(S,\mathscr{S},m)$ if the set

[TABLE]

is measurable and has measure zero. That is $m(N)=0$.

Notice that SR1-SR3 are slightly different than the axioms of a topology making measure spaces a similar, but not identical structure. In particular, one could say that a topology is "almost" a $\sigma$-algebra. There is a natural $\sigma$-algebra associated to every topology. This is known as the Borel $\sigma$-algebra composed of Borel sets. A Borel set is any subset of a topological space that can be constructed from countable intersections, countable unions, and complements of elements of the topology.

With the addition of a measure, a Borel $\sigma$-algebra on a topological space becomes a measure space. Borel measure spaces are of little use without a few regularity properties. First, a Borel measure is locally finite if every point has a neighborhood with finite measure. Second, a Borel measure space $(X,\mathscr{X},m_{x})$ is inner regular if for every $\varepsilon>0$ and $S\in\mathscr{X}$, there exists a compact $K\subset S$ such that $m_{X}(S\setminus K)<\varepsilon$.

A function $f$ from a measure space $(X,\mathscr{X},m_{X})$ into a measure space $(Y,\mathscr{Y},m_{Y})$ is said to be a measurable function if the preimage of every measurable set in $Y$ is a measurable set in $X$. That is

[TABLE]

Note that this is identical to the definition of continuity in a topological space if we replace the term "measurable" with "open". In light of the similarity between measure spaces and topological spaces, one might guess that a measurable function between Borel $\sigma$-algebra is "almost" a continuous function with respect to the topologies generating the algebras. This is indeed the case and this beautiful connection between measure theory and topology is due to Lusin [lusin1912proprietes].

Theorem C.1.1** (Lusin).**

Let $f$ be a measurable function from a locally finite, inner regular Borel measure space $(X,\mathscr{X},m_{X})$ into a Borel measure space $(Y,\mathscr{Y},m_{Y})$ generated by a second countable topology. For every $\varepsilon>0$ and $X$ in $\mathscr{X}$, there is a subset of $V$ of $X$ such that

$f$* is continuous on $X\setminus V$, and* 2. 2.

$m_{X}(V)<\varepsilon$.

There are many proofs of this theorem. This presentation of the proof is an expanded version of Feldman’s [feldman1981proof] with added detail.

Proof.

Since $\mathcal{T}_{Y}$ is second countable, it has a countable base $\{W_{i}\}_{i\in\mathbb{N}}$. By the inner regularity of $m_{X}$, there exists open sets $U_{i}$ in $\mathcal{T}_{X}$ such that $f^{-1}(W_{i})\subset U_{i}$ and $m_{X}(U_{i}\setminus f^{-1}(W_{i}))<\varepsilon/(2^{i})$ for each $i\in\mathbb{N}$. Now define the set

[TABLE]

It follows from Axiom SA5 of a measure space that $m_{X}$ is sub-additive so that

[TABLE]

Then from the construction of the sets $U$ we have

[TABLE]

The next step in the proof is to show that $f^{-1}(W_{i})\setminus V=U_{i}\setminus V$. First, it is an immediate consequence of the construction of $U_{i}$ that

[TABLE]

The reverse inclusion is derived as follows111It may be helpful to review some the elementary identities of set arithmetic in Appendix A.:

[TABLE]

Note that the last step follows from the construction of $U_{i}$. Intersecting the left and right hand sides of C.1.7 yields the desired reverse inclusion. Combining the above two equations, we conclude that

[TABLE]

Next, we use (C.1.8) to prove that $f$ is continuous on $X\setminus V$. Let $W$ be an open subset of $Y$ such that $f^{-1}(W)\cap V=\emptyset$. The set $W$ can be written as a union of elements of the base $W_{i}$

[TABLE]

Then

[TABLE]

Since $U_{i}$ are open in $\mathcal{T}_{X}$, the inverse image $(\bigcup_{i\in\mathcal{I}}U_{i})\setminus V$ is open in the subspace topology induced by $X\setminus V$. Therefore, $f$ is continuous on $X\setminus V$ which concludes the proof. ∎

C.1.1 Lebesgue Integration

Let $(X,\mathscr{X},m_{X})$ be a measure space. The indicator function $I_{S}:X\rightarrow\{0,1\}$ on a measurable set $S$ is defined as follows:

[TABLE]

A simple function $s:X\rightarrow[0,\infty)$ has the following form

[TABLE]

Now let $\mathcal{S}$ be the set of simple functions which are point-wise less than or equal to a nonnegative measurable function $f:X\rightarrow[0,\infty)$. The Lebesgue integral of $f$ is defined by

[TABLE]

The Lebesgue integral of a general real-valued measurable function is defined by

[TABLE]

where the two nonnegative functions on the right hand side are defined as in (C.1.13). A measurable function $f$ is said to be Lebesgue integrable if

[TABLE]

The Lebesgue integral has the following properties,

If $f,g$ are integrable and $\alpha,\beta$ are real numbers, then

[TABLE] 2. 2.

If $f,g$ are integrable and $f(x)\leq g(x)$ for all $x$ in $X$, then

[TABLE]

The set of integrable functions denoted $\mathcal{L}^{1}(X,\mathscr{X},m_{X})$ is a vector space. A semi-norm on this space is given by

[TABLE]

The vector space denoted ${L}^{1}(X,\mathscr{X},m_{X})$ refers to the quotient space of $\mathcal{L}^{1}(X,\mathscr{X},m_{X})$ with the Kernel of its semi-norm making the quotient space a proper normed space. In fact, it is a Banach space.

C.2 Integral Equations (Dynamical System Models)

Integral equations of the form

[TABLE]

appear frequently in engineering and the sciences when modeling various phenomena. The function $f:\mathbb{R}^{n}\times\mathbb{R}$ represents a model of some dynamical phenomenon, and the function $x:[t_{0},t_{f}]\rightarrow\mathbb{R}^{n}$ describes the evolution of a system parameterized by a vector in $\mathbb{R}^{n}$. For example, the laws of classical mechanics (pretty important for robotics) are stated as equations in the form of (C.2.1) or the analogous differential form.

It is therefore important to be confident that

this equation has a solution, 2. 2.

this solution is unique, 3. 3.

and the solution is continuous.

If there were instances with no solution then the framework would have little scientific value for modeling dynamical phenomena. If the second point failed then the model would not be useful for predicting the future state of the system. Lastly, if the solution to (C.2.1) were not continuous then solutions would permit instantaneous changes in the parameterization of a system. Taking these points into consideration highlights the importance and elegance of the fundamental existence-uniqueness theorem stated next.

Theorem C.2.1**.**

Let $f$ be Lipschitz continuous in its first argument on $\mathbb{R}^{n}$ and measurable in its second argument. Then there exists a unique solution $x:[t_{0},t_{f}]\rightarrow\mathbb{R}^{n}$ in $(\mathscr{C},\|\cdot\|_{\infty})$.

Proof.

The proof technique is known as the Picard iteration. Let $L$ denote a Lipschitz constant for $f$. Consider the mapping from one function $x:[t_{0},t_{0}+1/(2L)]\rightarrow\mathbb{R}^{n}$ to another function $y:[t_{0},t_{0}+1/(2L)]\rightarrow\mathbb{R}^{n}$ given by

[TABLE]

Observe that this mapping is well defined for all $x\in\mathscr{C}$ since the assumptions of the theorem are sufficient for the integral in (C.2.2) to be finite for each $\tau$. Further, the integral is continuous with respect to $\tau$ so that $y\in\mathscr{C}$ as well222We have tacitly assumed that $\mu$ is the Lebesgue measure on $\mathbb{R}$..

Denote the mapping from $x$ to $y$ by $y=\mathcal{P}(x)$. Notice that $x$ is a fixed point of $\mathcal{P}$ if and only if it is a solution to (C.2.2) with $t_{f}=1/(2L)$.

We will show that the Picard iterate $\mathcal{P}$ is a contraction on $\mathscr{C}$ to reach the result. Choose $x_{1},x_{2}\in\mathscr{C}$ with the domain $[t_{0},t_{0}+1/(2L)]$. Then

[TABLE]

Then leveraging the Lipschitz continuity of $f$ in its first argument, there exists an $L$ such that

[TABLE]

Thus, we have that

[TABLE]

which is equivalent to

[TABLE]

Therefore, the Picard iterate is a contraction on the interval $[t_{0},t_{0}+1/(2L)]$. By Theorem B.2.1 there exists a unique fixed point to the Picard iterate, and therefore there is a unique solution $x_{1}^{*}$ to (C.2.1) on the interval $[t_{0},t_{0}+1/(2L)]$. This solution necessarily satisfies $x_{1}^{*}(t_{0})=x_{0}$.

Now consider solutions on the time interval $[t_{0}+1/(2L),t_{0}+1/L]$ with initial condition $x^{*}_{1}(t_{0}+1/(2L))$, replacing $x_{0}$ in (C.2.1). By the same argument, there exists a unique solution $x_{2}^{*}$ on this time interval with $x^{*}_{1}(t_{0}+1/(2L))=x^{*}_{2}(t_{0}+1/(2L))$. Combining these observations, there is necessarily a unique continuous solution on the combined interval $[t_{0},t_{0}+1/L]$. A trivial induction argument leads to the conclusion that there is a unique continuous solution on any finite time interval $[t_{0},t_{f}]$. ∎

Next we show that if the dynamic model is bounded by $M$ as in Chapter 2, then the solutions are not only continuous, but Lipschitz continuous with Lipschitz constant $M$.

Corollary C.2.1**.**

If, in addition to the assumptions of Theorem 2.1.1, the function $f$ is bounded by $M$; then there is a unique Lipschitz continuous solution to (C.2.1) with Lipschitz constant $M$.

Proof.

Let $x$ be a solution to (C.2.1) on a finite time interval. Then for two times $t_{1}<t_{2}$ in this time interval,

[TABLE]

Thus, $x$ is Lipschitz continuous with constant $M$. ∎

Lemma C.2.1** (Gronwall’s Inequality).**

Suppose $z$ is a continuous function from the interval $[0,T]$ into $\mathbb{R}$. If

[TABLE]

for $z_{0},\lambda\in\mathbb{R}$, then

[TABLE]

Proof.

Let $y(t)=\int_{[0,t]}\lambda z(\tau)\,d\mu(\tau)$. The assumption that $z$ is continuous implies that $y$ is differentiable. Now let $w(t)=z_{0}+y(t)-z(t)$. We have $w(t)\geq 0$. Rewriting the definition of $y$ in terms of $w(t)$ yields,

[TABLE]

Taking the derivative of $y$ yields

[TABLE]

This is a linear differential equation with solution

[TABLE]

Since $w(t)\geq 0$ we have the inequality

[TABLE]

Returning to the definition of $y(t)$ we have

[TABLE]

which is Gronwall’s inequality. ∎

Gronwall’s inequality is best known for its use a a Lemma in proving the continuity of solutions to (C.2.1) with respect to the initial condition $x_{0}$. Continuity with respect to the initial conditions adds to the value of the (C.2.1) as a modeling framework for dynamical systems. Without it, an arbitrarily small change in initial conditions could lead to drastically different outcomes.

Theorem C.2.2**.**

If the assumptions of Theorem 2.1.1 are met, the the terminal state of the solution to (C.2.1) is Lipschitz continuous with respect to the initial condition $x_{0}$.

Proof.

Suppose $x_{1}$ and $x_{2}$ are solutions to two instances of (C.2.1) on the time interval $[t_{0},t_{f}]$ with different initial conditions. Then

[TABLE]

As a direct consequence of Lemma C.2.1 we have that

[TABLE]

∎

Appendix D Graph Search Algorithms

This appendix provides an introduction to shortest path problems on graphs and an informed best-first search, also known as the $A^{*}$ algorithm.

A graph is a set $V$ equipped with a binary relation $E$ on $V\times V$. An element of $V$ is called a vertex and an element of $E$ is called an edge. A path $\{p_{1},...,p_{n}\}$ is a finite sequence in $V$ such that $(p_{i},p_{i+1})\in E$ for each sequential pair in the sequence. A weighted graph is a graph equipped with a function $c:E\rightarrow\mathbb{R}$ which assigns a weight or cost to each edge. The cost or weight of a path $\{p_{i}\}$ with length $n$ in a weighed graph is, with some abuse of notation, given by

[TABLE]

A shortest path from a source $s\in V$ to a destination set $D\subset V$ is a finite path $\{p_{1},...,p_{n}\}$ with $p_{1}=s$, $p_{n}\in D$, and which has the minimum cost among paths originating from $s$ and terminating in $D$.

Lemma D.0.1**.**

A finite, weighted graph with non-negative edge weights either admits a shortest path from $s\in V$ to $D\subset V$ with cost $c^{*}$, or has no path from $s$ to $D$.

Proof.

Suppose there is a path $\{p_{i}\}$ from $s$ to $D$. Then the set of paths from $s$ to $D$ is nonempty. Note that for every path that visits a vertex twice, there exists a path with equal or lesser cost that omits the cycle. Therefore, if a path from $s$ to $D$ exists, then there is a nonempty set of paths from $s$ to $D$ with no cycles. A path with no cycles is bounded in length by the number of vertices in the graph $N$, and the number of paths of length $N$ is bounded by $N^{N}$. Thus, there are a finite number of paths with no cycles. Since every real valued function with a finite domain admits a minimum, there is a minimum cost path within this subset which lower bounds all paths from $s$ to $D$. ∎

The $A^{*}$ algorithm is generally the most effective algorithm for single-source shortest path problems. An essential component of the $A^{*}$ algorithm is an admissible heuristic; a function $h:V\rightarrow\mathbb{R}$ which estimates the cost of a shortest path from each vertex to $D$, and never overestimates it. The algorithm incrementally examines paths originating from $s$ in order of their cost plus the estimated cost to reach $D$. The algorithm uses a function $\texttt{label}:V\rightarrow\mathbb{R}\cup\{\infty\}$ which is recursively redefined at each iteration with the cost of the best known path reaching each vertex; the function initially maps all vertices to $\infty$ representing that no path from $s$ to any particular vertex is known. A function $\texttt{parent}:V\rightarrow V\cup\{\mathtt{NULL}\}$ is redefined in each iteration to implicitly build a secondary graph $(V\cup\{\mathtt{NULL}\},\tilde{E})$ of shortest paths from $s$ to every other vertex. The edge set of this graph is

[TABLE]

The function parent initially maps all vertices to $\mathtt{NULL}$ which represents that no path to these vertices is known.

A set $Q$ of vertices, associated to paths from $s$ by the parent function, is operated on by the algorithm with standard set operations together with an operation pop which returns a vertex $v\in Q$ with terminal vertex satisfying

[TABLE]

The last operation that is needed to present the algorithm is a function $\texttt{neighborhood}:V\rightarrow 2^{E}$ which returns the set of edges in the graph whose first component is the argument of the function. That is,

[TABLE]

In each iteration of the algorithm, an element of $Q$ with greatest merit is removed from $Q$ and assigned to $v$ (line 5). The algorithm will terminate if $v\in D$ since the path $(v_{1},v_{2}...,v_{n})$ satisfying

[TABLE]

is a path from $s$ to $D$. If $v$ is not in the destination set, then the algorithm will continue the search by exploring extensions of the subgraph implicitly defined by the parent function (line 7 - line 12). This is accomplished by assigning the neighbors of $v$ to the set $S$ (line 7). Each of the vertices in $S$ can be reached via the path to $v$ together with the edge connecting $v$ to each vertex in $S$. The cost of these paths is the cost to reach $v$, accesed by $\texttt{label}(v)$, plus the cost of the edge from $v$ to the vertex in $S$. If this cost is less than the current label of any of the vertices in $S$, the function label is updated with that lower cost (line 10). Additionally, the path to this vertex in $S$ is stored by assigning the parent of that vertex to $v$ (line 11). The last step in each iteration is to add the vertices of $S$ whose label was updated to the set $Q$ for examination in future iterations. The algorithm terminates when a path from $s$ to $D$ is found as discussed above, or when the set $Q$ is empty and there are no more options for finding better paths than has already been discovered.

Theorem D.0.1**.**

On a finite weighted graph with nonnegative edge weights, the $A^{*}$ algorithm terminates in finite time returning a shortest path if one exists, or returns $\mathtt{NO\,SOLUTION}$ if there is no path from $s$ to $D$.

Proof.

(Finite running time) During the loop (line 3 - line 12), a vertex is inserted into $Q$ if a lower cost path to that vertex than was previously known was discovered. Since the number of paths with cost less than any particular value is finite, the insertion of any vertex into $Q$ can only occur a finite number of times. Together with the finite number of vertices, the total number of vertices inserted into $Q$ during execution is finite. In each iteration, one vertex is removed from $Q$. Therefore, the algorithm must eventually terminate by returning NO SOLUTION or a vertex and the parent function.

(Optimality of returned path) Let $\{v_{1},...,v_{n}\}$ be a shortest path from $s$ to $D$ with cost $c(\{v_{1},...,v_{n}\})=c^{*}$. Suppose, as a point of contradiction, that the algorithm does not return a path with cost $c^{*}$. Then $v_{n}$ is never present in $Q$ with $\texttt{label}(v_{n})=c(\{v_{1},...,v_{n}\})$; the reason being that, if it were inserted into $Q$ with this label, then

[TABLE]

Then, prior to termination, $\texttt{pop}(Q)$ would return $v_{n}\in D$ at which point the algorithm would terminate with a shortest path—contradictory our hypothesis. It follows that $v_{n-1}$ never enters $Q$ with $\texttt{label}(v_{n-1})=c(\{v_{1},...,v_{n-1}\})$. Similar to the previous step, we would have

[TABLE]

so $\texttt{pop}(Q)$ would return $v_{n-1}$ with this label before termination of the algorithm. Since $v_{n}$ is a neighbor of $v_{n-1}$ this would result in setting $\texttt{label}(v_{n})$ to $c(\{v_{1},...,v_{n-1}\})+c(\{v_{n-1},v_{n}\})$ in line 10 which, as was previously established, does not occur. Continuing these deductive steps leads to the conclusion that $\{v_{1}\}$ must never enter $Q$ with $\texttt{label}(v_{1})=c(\{v_{1}\})=0$. This is a contradiction of line 2 of the algorithm. Thus, if a path from $s$ to $D$ exists, the algorithm must terminate, returning a vertex $v_{n}\in D$ with $\texttt{label}(v_{n})=c^{*}$. A shortest path from $v_{1}=s$ to $v_{n}\in D$ can then be recovered with the parent function.

In the event that there is no path from $s$ to $D$, the algorithm will never insert a vertex $v\in D$ into $Q$ since such vertices are always associated to a path to $s$ through the parent function. Since termination of the algorithm was already established, it must terminate with $\mathtt{NO\,SOLUTION}$.

∎

Appendix E Extended Derivations and Proofs

This appendix contains the tedious and less interesting proofs that are necessary for the completeness of this thesis.

E.1 Proof that $d_{\mathcal{U}}$ is a Metric on $\mathcal{U}$

Recall the the proposed metric $d_{\mathcal{U}}$ for the input signal space $\mathcal{U}$ in Chapter 3,

[TABLE]

The following derivation shows that $d_{\mathcal{U}}$ satisfies axioms MS1-MS4 for a metric: Let $u_{1}$, $u_{2}$, and $u_{3}$ denote elements of $\mathcal{U}$.

(MS1) By the nonnegativity of the norms on $\mathbb{R}$ and $\mathbb{R}^{n}$, the integrand $\|u_{1}(t)-u_{2}(t)\|_{2}$ and $|\tau(u_{1})-\tau(u_{2})|$ are nonnegative for all times $t$. Thus,

[TABLE]

so that MS1 is satisfied.

(MS2) Consider two input signals $u_{1}$, $u_{2}$ which are not equal. Then at least one of the following is true: (i) their time domains differ so that $|\tau(u_{1})-\tau(u_{2})|>0$, (ii) there is a subset of $S\subset[0,\min\{\tau(u_{1}),\tau(u_{2})\}]$ on which $u_{1}(t)\neq u_{2}(t)$ in which case $\|u_{1}(t)-u_{2}(t)\|_{2}>0$. For the latter case, recall that the $L_{1}$-spaces are quotient spaces whose elements are the equivalence classes of signals that differ only on sets of measure zero. Thus, $S$ has positive measure. Then,

[TABLE]

In the case that $u_{1}=u_{2}$, we have $u_{1}(t)=u_{2}(t)$ for every $t$ in $[0,\tau(u_{1})]$. Additionally, $\tau(u_{1})=\tau(u_{2})$ so the distance between $u_{1}$ and $u_{2}$ is zero when they are equal. Thus, MS2 is satisfied.

(MS3) Next, symmetry of the metric follows from the symmetry of the Euclidean metric from which it is constructed,

[TABLE]

so that MS3 is satisfied.

(MS4) To simplify the notation of the proof, let $\tau(u_{1})\leq\tau(u_{2})\leq\tau(u_{3})$. To show that triangle inequality holds together with this assumption, for any three points in $\mathcal{U}$ it will need to be shown for all permutations of $u_{1}$, $u_{2}$, $u_{3}$. From the triangle inequality for the Euclidean metric we have,

[TABLE]

Note that in the second step above, the integration in the second term is carried out over a larger domain to arrive at the desired result. The next permutation is nearly identical to the first,

[TABLE]

Now the third permutation,

[TABLE]

The remaining three cases follow trivially from the symmetry property already established. Thus, the function $d_{\mathcal{U}}$ is a metric on the set of input signals $\mathcal{U}$.

E.2 Proof that ${d_{\mathcal{X}}}$ is a metric on $\mathcal{X}$

Recall the the proposed metric $d_{\mathcal{X}}$ for the input signal space $\mathcal{X}$ in Chapter 3,

[TABLE]

The following derivation shows that $d_{\mathcal{X}}$ satisfies axioms MS1-MS4 for a metric: Let $x_{1}$, $x_{2}$, and $x_{3}$ be elements of $\mathcal{X}$.

(MS1) For each $t\in\left[0,\min\{\tau(x_{1}),\tau(x_{2})\}\right]$ the term $+M|\tau(x_{1})-\tau(x_{2})|$ is nonnegative as is $\left\|x_{1}(t)-x_{2}(t)\right\|_{2}$. The maximum over a set of nonnegative numbers is nonnegative so $d_{\mathcal{X}}$ is nonnegative.

(MS2) Suppose $d_{\mathcal{X}}(x_{1},x_{2})=0$. Then

[TABLE]

Since the expression is a sum of two nonnegative terms adding to zero, each term is necessarily zero. Therefore, $\tau(x_{1})=\tau(x_{2})$, and $x_{1}(t)=x_{2}(t)$ for all $t\in\left[0,\tau(x_{1})\right]$ and $t\in\left[0,\tau(x_{2})\right]$. Equivalently, $x_{1}=x_{2}$. Now suppose $x_{1}=x_{2}$. Then $\tau(x_{1})=\tau(x_{1})$ and $x_{1}(t)=x_{2}(t)$ for every $t$ in $[0,\tau(x_{1})$ and $[0,\tau(x_{2})]$. This implies $|\tau(x_{1})-\tau(x_{1})|=0$ and $\|x_{1}(t)-x_{2}(t)\|_{2}=0$. Therefore, $d_{\mathcal{X}}(x_{1},x_{2})=0$.

(MS3) Symmetry of the distance function is inherited from the symmetry of the metrics from which it is constructed,

[TABLE]

(MS4) To simplify the notation, assume $\tau(x_{1})\leq\tau(x_{2})\leq\tau(x_{3})$. To show that triangle inequality holds together with this assumption, the inequality it will have to be shown for all permutations of $x_{1}$, $x_{2}$, $x_{3}$: First,

[TABLE]

Next,

[TABLE]

Then,

[TABLE]

The remaining three cases follow trivially from the symmetry property already established.

E.3 Derivation of Inequality (5.4.2)

This section derives the inequality

[TABLE]

First, we factor out the term $\sqrt{n}/\eta(R)$,

[TABLE]

followed by reversing the order of the summation,

[TABLE]

Note that for a general nondecreasing function $f:\mathbb{R}\rightarrow\mathbb{R}$ we have the inequality

[TABLE]

since the sum is the left-Riemann sum if the integral. This can be applied to (E.3.3) to obtain

[TABLE]

which is the desired inequality.

Index

• admissible control inputs Chapter 2
• admissible heuristic Appendix D, footnote 1, §6.1.1
• admissible states Chapter 2
• almost everywhere §C.1
• attraction sequence §2.3.1
• Banach space §B.3.1
• base §B.1
• binary relation §A.3
• Borel set §C.1
• Borel $\sigma$-algebra §C.1
• Cauchy sequence §B.2.2
• closed set §B.1
• closure §B.1
• codomain §A.3.1
• compact sets §B.1.1
• complement §A.2
• complete metric space §B.2.2
• composition relation §A.3
• consistent heuristic §6.1.1
• continuous §B.1.2
• contraction §B.2.2
• convergent sequence §B.1
• cover §A.2
• disjoint §A.2
• dispersion Chapter 4
• domain §A.3.1
• edge (graph) Appendix D
• empty set §A.1
• equal §A.1
• equivalence class §A.3.3
• equivalence relation §A.3.3
• expansiveness §2.3.1
• fixed point §A.3.1
• function §A.3.1
• GLC conditions §5.1
• goal states Chapter 2
• graph Appendix D
• greatest lower bound property §A.3.2
• Hamilton-Jacobi-Bellman equation Chapter 6
• heuristic §6.1.1
• indicator function §C.1.1
• infimum §A.3.2
• initial condition §2.2
• inner regular measure spaces §C.1
• input signal Chapter 2
• interior §B.1
• intersection §A.2
• inverse relation §A.3
• invertible function §A.3.1
• kinodynamic motion planning §1.1.3
• label correcting method Chapter 5
• least upper bound property §A.3.2
• Lebesgue integration §C.1.1
• limit point §B.1
• Lipschitz continuity §B.2.1
• locally finite measure space §C.1
• lower bound §A.3.2
• measurable function §C.1
• measurable set §C.1
• measure §C.1
• metric §B.2
• metric space §B.2
• metric subspace §B.2
• minimal (partial order) §5.1
• neighborhood §B.1
• norm §B.3
• normed vector space §B.3
• open ball §B.2
• open set §B.1
• order §A.3.2
• partial order §A.3.2
• partition §A.2
• path (graph) Appendix D
• power set §A.1
• principle of optimality §5.2
• probabilistic completeness §1.1.3
• problem data §2.3
• problem relaxation §6.2.1
• relaxed optimal kinodynamic motion planning problem §2.2
• resolution complete §2.2
• resolution completeness §1.1.3
• running cost §2.2
• scalars §B.3
• second countable topological space §B.1
• sequence §A.3.1
• set exclusion §A.2
• shortest path (graph) Appendix D
• simple function §C.1.1
• solution (integral equation) §2.1
• strict partial order §A.3.2
• subcover §A.2
• subsequence §A.3.1
• subset §A.1
• subspace topology §B.1
• supremum §A.3.2
• system map §2.1
• terminal control input §2.1
• terminal state §2.1
• terminal time §2.1
• topological space §B.1
• topology §B.1
• trajectory Chapter 2
• uniform continuity §B.2.1
• union §A.2
• upper bound §A.3.2
• value function Chapter 6
• vector space §B.3
• vectors §B.3
• vertex Appendix D
• viscosity solution footnote 1
• $\delta$-robust feasibility §2.3.1
• $\delta$-robustness §2.3.1
• $\varepsilon$-goodness §2.3.1
• $\sigma$-algebra §C.1