diff --git a/lecture_15/fig15_1.PNG b/lecture_15/fig15_1.PNG new file mode 100644 index 0000000..9f3f817 Binary files /dev/null and b/lecture_15/fig15_1.PNG differ diff --git a/lecture_15/fig15_10.PNG b/lecture_15/fig15_10.PNG new file mode 100644 index 0000000..f85059f Binary files /dev/null and b/lecture_15/fig15_10.PNG differ diff --git a/lecture_15/fig15_11.PNG b/lecture_15/fig15_11.PNG new file mode 100644 index 0000000..6153f6c Binary files /dev/null and b/lecture_15/fig15_11.PNG differ diff --git a/lecture_15/fig15_12.PNG b/lecture_15/fig15_12.PNG new file mode 100644 index 0000000..6f4820d Binary files /dev/null and b/lecture_15/fig15_12.PNG differ diff --git a/lecture_15/fig15_2.PNG b/lecture_15/fig15_2.PNG new file mode 100644 index 0000000..e1d6a85 Binary files /dev/null and b/lecture_15/fig15_2.PNG differ diff --git a/lecture_15/fig15_3.PNG b/lecture_15/fig15_3.PNG new file mode 100644 index 0000000..d271e1a Binary files /dev/null and b/lecture_15/fig15_3.PNG differ diff --git a/lecture_15/fig15_4.PNG b/lecture_15/fig15_4.PNG new file mode 100644 index 0000000..b3b8ba9 Binary files /dev/null and b/lecture_15/fig15_4.PNG differ diff --git a/lecture_15/fig15_5.PNG b/lecture_15/fig15_5.PNG new file mode 100644 index 0000000..76ef952 Binary files /dev/null and b/lecture_15/fig15_5.PNG differ diff --git a/lecture_15/fig15_6.PNG b/lecture_15/fig15_6.PNG new file mode 100644 index 0000000..eeb8a14 Binary files /dev/null and b/lecture_15/fig15_6.PNG differ diff --git a/lecture_15/fig15_7.PNG b/lecture_15/fig15_7.PNG new file mode 100644 index 0000000..41c9fcd Binary files /dev/null and b/lecture_15/fig15_7.PNG differ diff --git a/lecture_15/fig15_8.PNG b/lecture_15/fig15_8.PNG new file mode 100644 index 0000000..46446c1 Binary files /dev/null and b/lecture_15/fig15_8.PNG differ diff --git a/lecture_15/fig15_9.PNG b/lecture_15/fig15_9.PNG new file mode 100644 index 0000000..eb3ca28 Binary files /dev/null and b/lecture_15/fig15_9.PNG differ diff --git a/lecture_15/forbidden_space.jpeg b/lecture_15/forbidden_space.jpeg new file mode 100644 index 0000000..2f67192 Binary files /dev/null and b/lecture_15/forbidden_space.jpeg differ diff --git a/lecture_15/forbidden_space_robot_obstacle.jpeg b/lecture_15/forbidden_space_robot_obstacle.jpeg new file mode 100644 index 0000000..7313520 Binary files /dev/null and b/lecture_15/forbidden_space_robot_obstacle.jpeg differ diff --git a/lecture_15/lecture_13_biblio.bib b/lecture_15/lecture_13_biblio.bib deleted file mode 100644 index c383b03..0000000 --- a/lecture_15/lecture_13_biblio.bib +++ /dev/null @@ -1,15 +0,0 @@ -@book{thrun2005probabilistic, - title={Probabilistic Robotics}, - author={Thrun, Sebastian and Burgard, Wolfram and Fox, Dieter}, - year={2005}, - publisher={MIT press} -} - -@misc{Xmisc, - author = {Pavone, Marco}, - title = {AA 274 Principles of Robotic AutonomyLocalization II: parametric and non-parametric filters}, - howpublished = {Online}, - year = {2018}, - month = {February}, - note = {Presentation}, -} \ No newline at end of file diff --git a/lecture_15/lecture_15_biblio.bib b/lecture_15/lecture_15_biblio.bib new file mode 100644 index 0000000..4f5a192 --- /dev/null +++ b/lecture_15/lecture_15_biblio.bib @@ -0,0 +1,35 @@ +@book{thrun2005probabilistic, + title={Probabilistic Robotics}, + author={Thrun, Sebastian and Burgard, Wolfram and Fox, Dieter}, + year={2005}, + publisher={MIT press} +} + +@misc{pavone, + author = {Pavone, Marco}, + title = {AA 274 Principles of Robotic Autonomy Lecture 15: Combinatorial Motion Planning}, + howpublished = {Online}, + year = {2018}, + month = {February}, + note = {Presentation}, +} + +@inbook{siegwart, + title={Introduction to Autonomous Mobile Robots}, + author={Roland Siegwart}, + series={2rd ed.}, + chapter = "Section 6.3", +} + +@inbook{bertsekas, + title={Dynamic Programming and Optimal Control}, + author={Dimitri Bertsekas}, + series={Vol I, 3rd ed}, + chapter = "Section 2.3", +} + +@inbook{lavalle, + title={Planning Algorithms}, + author={Steven M. Lavalle}, + chapter = "Section 6.1-6.3, 6.5", +} \ No newline at end of file diff --git a/lecture_15/main.pdf b/lecture_15/main.pdf index 6aaab87..266f6d6 100644 Binary files a/lecture_15/main.pdf and b/lecture_15/main.pdf differ diff --git a/lecture_15/main.tex b/lecture_15/main.tex index 35614b9..9c17b59 100644 --- a/lecture_15/main.tex +++ b/lecture_15/main.tex @@ -12,7 +12,13 @@ \usepackage[autostyle]{csquotes} \usepackage{placeins} \usepackage[backend=biber]{biblatex} -\bibliography{./lecture_13_biblio.bib} +\bibliography{./lecture_15_biblio.bib} + +\usepackage{tikz,pgfplots} +\usepackage{xcolor, colortbl} +\usepackage{array} +\usepackage[boxruled]{algorithm2e} + \usepackage{graphics} \usepackage{graphicx} @@ -27,6 +33,7 @@ \setlength{\headsep}{0.75 in} \setlength{\parindent}{0 in} \setlength{\parskip}{0.1 in} +\DeclareMathOperator*{\argmin}{arg\,min} \newcounter{lecnum} @@ -65,22 +72,490 @@ %%%%%%%%%%%%%%%%%%%%%%%%%% %document \begin{document} -%modify this +\nocite{pavone} +\nocite{siegwart} +\nocite{bertsekas} +\nocite{lavalle} + \lecture{15}{Combinatorial Motion Planning}{} -\section{Introduction} +\section{Introduction} \label{Introduction} + +A robot must show competence in reaching its goal position as efficiently as possible based on its knowledge and sensor values given a series of goal positions and a map of its environment. This task gives rise to the navigation or motion planning problem. + +\textbf{Robot motion planning}: compute a sequence of actions that drives a robot from an initial condition to a terminal condition while avoiding obstacles, satisfying kinematic and dynamic constraints, and possibly optimizing an objective function. + +Understanding motion planning is very critical as it plays a major role in some common motion planning problems such as steering autonomous vehicles, controlling humanoid robots, protein docking, and surgery planning. + +In these notes, we will explore several key aspects of motion planning. In Section \ref{Introduction}, we will go into what motion planning is and the history behind it. In Section \ref{Configuration-Space}, we dive deeper into motion planning, exploring a configuration space in a 2D example and also how we plan by mapping out free and forbidden spaces. In Section \ref{Combinatorial Planning}, we will discuss one type of motion planning solution called combinatorial planning which does not make any approximations in the configuration space. In order for combinatorial planning to be possible, we must understand how to go about determining the roadmap a robot or agent can take given a map of the free space. This is accomplished via cell decomposition and explained in Section \ref{Cell Decomposition}. Another approach to motion planning is a grid based solution, which discretizies and approximates the world into grid blocks. In Section \ref{Graph-Search Algorithms} we explore graph-based solutions for finding the shortest paths between the start and end points such as Depth-First Search, Breadth-First Search, Dijkstra, and the more general Label Correcting Algorithm. Finally in Section \ref{Correctness and Improvements}, we will dive into a more efficient graph search algorithm which uses a heuristic function in order to search the grid state space more efficiently called A*. + +\begin{itemize} + \item Introduction and History of Motion Planning + \item Configuration Space + \item Combinatorial Planning + \item Cell-Decomposition + \item Graph-Search Algorithms + \item Correctness and Improvements + +\end{itemize} + + +\subsection{ History } + +The problem of motion planning was formally defined in the 1970s as mobile robots became more prevalent in industries. However, exact combinatorial solutions in discrete configuration spaces were not developed until the 1980s with the introduction of Breadth-First Search and Depth-First Search algorithms. Grid-based search algorithms such as A* and D* which was developed by Anthony Stentz followed in the mid-1990s \cite{bertsekas}. These methods worked well for simple motion planning problems but were subject to the curse of dimensionality for planning in high-dimensional state spaces. To address this challenge, +sampling-based methods, notably Probability Road-Maps (PRM) and Rapidly-Exploring Random Trees (RRT), were developed in the late 1990s and deployed on real-time systems in the 2000s \cite{lavalle}. With the increasing complexity of robotic systems, solving motion planning problems in real-time has become a significant challenge. Hence, current research has focused on speeding up sampling-based methods through lazy dynamic programming and massive parallelization of search algorithms. + +\section{Configuration-Space} \label{Configuration-Space} +In the context of robotic motion planning, we will first create a simple problem setup to define the variables of interest and introduce the proper nomenclature. First, consider a \textit{workspace} defined in 2D, $ \mathcal{W} \subseteq \mathbb{R}^2$, which effectively describes the world that is occupied by the robot, the obstacles, and potentially other agents in the system. Within this workspace, we define the robot as a rigid polygon, and we introduce the set of obstacles $\mathcal{O} \subset \mathcal{W}$ as a subset of the workspace shown in Fig 15.1. + +The rigid robot can be translated or rotated according to its own kinematic constraints. With these definitions established, we can qualitatively define the robotic motion planning problem as: + +\begin{figure}[ht!] +\begin{center} +\includegraphics{fig15_1.PNG} +\caption{2D Workspace} +\end{center} +\end{figure} + +\textbf{Problem Statement}: Compute the sequence of movements to go from a given initial placement of the robot to a desired goal placement such that the robot never touches the obstacle regions. + +While the robot motion planning problem is described in a physically intuitive ”real-world” setting (e.g., the 2D planar workspace described above), it is vital to map the problem into another space, denoted as the \textit{configuration} ($\mathcal{C-}$) space. The configuration space is simply the space containing the degrees of freedom of the robot. The degrees of freedom of the robot is defined as the smallest number, $d \geq 1$, of coordinates required to fully represent the configuration of the robot. Returning to our simple example highlighted above, the configuration space contains the translational degrees of freedom and the rotational degrees of freedom of the robot, totaling $d=3$ degrees of freedom. In this way, the 2D path planning problem in physical space is cast as a 3D path planning problem in the configuration space, with two dimensions describing the translational modes of the trajectory and one degree of freedom capturing the rotational mode of the trajectory in the planar workspace. + +Formally, the degrees of freedom for our rigid polygon robot, $\mathcal{R} \subset \mathbb{R}^2$, is the set of generalized coordinates, $q = (x_t, y_t, \theta)$, where $(x_t, y_t) \subset \mathbb{R}^2$ are the translational coordinates, and $\theta$ is the rotational coordinate. Accordingly, the configuration space consists of every combination of q which yields a unique robot placement (in this case a subset of $\mathbb{R}^3$). An important distinction must be made for the generalized coordinate describing the rotational degree of freedom; namely, $\theta$ and $\theta \pm 2\pi k, k \in \mathbb{Z}$ describe equivalent attitudes. The configuration space in this case is actually in $\mathbb{R}^2 \times \mathcal{S}^1$, where $\mathcal{S}^1$ is the manifold which contains the angular displacement variable and includes the aforementioned ”equivalence” between angles that differ by integer multiples of $2\pi$. If we relax the planar restriction in our working example, the angular orientation coordinates of the robot would instead be in the manifold $\mathcal{S}^3$. The importance of this distinction is highlighted with a simple example: consider a robot that has a current heading of $\theta = 3\pi/2$ rads, and we want to reconfigure the robot to heading $\theta_g = 0$ subject to the constraint of avoiding a $\mathcal{C}$-space obstacle at $\theta = \pi$. The geometry of this problem is highlighted in Figure 15.2. Now, if the equivalence between the angles 0 and $2\pi$ is not established in the definition of the configuration space, the robot would not be able to traverse a collision-free path to the desired heading in the configuration space (see red trajectory). Instead, since the configuration space is defined with respect to $\mathcal{S}^1$, the robot is able to achieve the desired heading (see green trajectory). + +\begin{figure}[ht!] +\begin{center} +\includegraphics[width=0.8\textwidth]{fig15_2.PNG} +\caption{Example trajectory planning where the rotational degree of freedom is not described on $\mathcal{S}^1$ (red) and where it is described on $\mathcal{S}^1$ (green)} +\end{center} +\end{figure} + +Note that for our simple planar workspace example, the dimensions of the physical space and the configuration space are quite similar. Instead, it is not uncommon for complicated dynamical systems (e.g., a robotic arm mounted on a spacecraft) to have configuration spaces of much higher dimension than the physical space counterpart. In these cases, mapping from physical space to configuration space is extremely vital to the motion planning problem as it allows all constraints of the complex kinematic system to be accounted for with higher-dimensional information (i.e., more coordinates). + +\subsection{ Planning in $\mathcal{C}$-Space } +Now that we have introduced the necessary nomenclature and addressed the important distinction between motion planning in the real (physical) space and the configuration space, we can return to our prior problem statement and re-write it in rigorous mathematical detail. Let the set of points in the workspace that are occupied by the robot in configuration $q$ be denoted by $\mathcal{R}(q) \subset \mathcal{W}$. Furthermore, we can make the intuitive definition of collision-free motion as that set of all robot configurations which do not intersect the obstacle regions: $\mathcal{R}(q)\cap \mathcal{O} = \emptyset$. Then the formal problem statement can be re-written as: + +\textbf{Problem Statement, Revisited:} \\ +Compute the continuous path: $\tau : [0, 1] \Rightarrow \{q \in \mathcal{C}|\mathcal{R}(q) \cap \mathcal{O} = \emptyset\}$ with $\tau(0) = q_1$ and $\tau(1) = q_G$ + + In general, we refer to the set $\{q \in \mathcal{C} | \mathcal{R}(q) \cap \mathcal{O} = \emptyset\}$ as $\mathcal{C}_{free}$, which represents all possible combinations of the robot’s degrees of freedom which are collision-free. For a simple 2 degree of freedom circle robot and triangle obstacle, as seen in Figure 15.3a, the forbidden space can be constructed by drawing a line through the center every robot configuration that just touches the obstacle. It can be seen that a forbidden robot state will be any state in which the center of the robot is inside the forbidden space. + + \begin{figure} + \centering + \begin{subfigure}[b]{0.35\textwidth} + \includegraphics[width=\textwidth]{forbidden_space_robot_obstacle} + \caption{Simple 2 DOF robot and obstacle in $\mathbb{R}^2$} + \end{subfigure} + \begin{subfigure}[b]{0.3\textwidth} + \includegraphics[width=\textwidth]{forbidden_space} + \caption{Forbidden space of simple robot and obstacle.} + \end{subfigure} +\end{figure} + + + A key fact here is that, while a robot occupies physical dimensions in the real space, the robotic motion is described by a simple point in the configuration space that captures the translation degrees of freedom (of the center of mass for example) and the rotational degrees of freedom of the robot. In this way, mapping to the configuration space poses the motion planning problem as a simpler problem of finding a path for a point. An illustration of this problem is highlighted in Figure 15.4. The physical dimensionality of the robot is intrinsically captured in the definition of the configuration space (in particular, in $\mathcal{C}_{free}$). This property represents a key advantage of moving to the configuration space for robotic motion planning, as it allows the same general planning problem formulation (i.e., path planning for a point) to be equally applicable to myriad dynamical systems. + +\begin{figure}[ht!] +\begin{center} +\includegraphics[width=0.4\textwidth]{fig15_3.PNG} +\caption{Robotic motion planning as a point path planning problem in C-space.} +\end{center} +\end{figure} + +We now have a formal mathematical definition for the robot motion planning problem. In order to simplify the procedure, we recast the problem from the real/physical space to the configuration space. The result is that the motion planning problem now becomes a path planning problem for a single point. We have not yet, however, set up an architecture for obtaining the point path solution. In the following sections, several solution methods will be highlighted which leverage the aforementioned advantages of working in the configuration space for motion planning. + +\section{Combinatorial Planning} \label{Combinatorial Planning} +The goal of combinatorial planning is to find paths through the configuration space without needing approximations. For example, Figure \ref{fig:15_4} represents a point robot in a plane. The key idea is to compute a roadmap, which is a graph in which each vertex is a configuration in $C_\text{free}$ and each edge is a path through $C_\text{free}$ that connects a pair of vertices. + +For low dimensional convex problems, this type of planning either provides \textit{exact} solutions or none if no feasible path exists. This can allow for better performance in some cases, compared to approximation-based methods. + +\begin{figure}[H] +\begin{center} +\includegraphics[width=0.6\textwidth]{fig15_4.PNG} +\caption{Combinatorial Planning} +\label{fig:15_4} +\end{center} +\end{figure} + +Combinatorial Planning refers to the use of all combinations of possible states in defining the robot configuration $q$. For example, consider the configurations used above in setting up the configuration-space: $q = (x_t, y_t, \theta)$. In this case, the $x$ and $y$ position and the heading $\theta$ define a unique configuration. + +Combinatorial motion planning approaches construct a roadmap, a graph $G$ which should preserve: + +\begin{enumerate} + \item \textbf{Accessiblity}: It is always possible to connect some $q$ to the roadmap. For instance, $q_I$ can be connected to $s_1$ and $q_G$ can be connected to $s_2$ on the roadmap. + \item \textbf{Connectivity}: If there is a path between two configurations $q_I$ and $q_G$, then there exists a path on the roadmap between $s_1$ and $s_2$. +\end{enumerate} + +The roadmap essentially provides a discrete representation of the continuous problem without losing any of the original connectivity information needed to solve it. + +The typical approach involves cell decomposition. There are requirements that the cells be easy to traverse, the decomposition be easy to compute, and the cell adjacencies be straightforward to determine. + +\section{Cell Decomposition} \label{Cell Decomposition} +In order to pick the roadmap the environment needs to be decomposed in a way where connectivity between subspaces of the environment is captured. The most widely used method for representing an environment is cell decomposition which entails making a discrete representation of the configuration space. Cell decomposition refers to a set algorithms that partitions $C_{free}$ into a finite set of regions called cells. Three properties should be satisfied by cell decomposition, to reduce motion planning problem to a graph search problem: + +\begin{enumerate} +\item Each cell should be easy to traverse and ideally convex. +\item Decomposition should be easy to compute. +\item Adjacencies between cells should be straightforward to determine, in order to build the roadmap. +\end{enumerate} + +\subsection{2D Decomposition} +We first take a look at 2D vertical cell decomposition, which partitions $C_{free}$ into a finite collection of 2-cells and 1-cells, where k-cell refers to a k-dimensional cell. Each 2-cell is either a trapezoid or a triangle. +Each 1-cell is a vertical segment that serves as the border of two 2-cells. Figure 15.5 shows an environment represented as a configuration space, with free configuration space, $C_{free}$, denoted as the white regions and obstacle space, $C_{obs}$, denoted as the dark regions. Though the planning problem is 2-dimensional, the robot configuration space is 3-dimensional, as the heading of the robot adds another degree of freedom. + +\begin{figure}[h] +\begin{center} +\includegraphics{fig15_5.PNG} +\caption{Example of 2D Cell Decomposition and Roadmap Extraction given $q_{I}$ and $q_{G}$.} +\end{center} +\end{figure} + +Assume the obstacle space is polygonal, then vertical cell decomposition works as follows. Let P denote the set of vertices used to define $C_{obs}$. For each vertex $p_{i} \in P$, extend rays upwards and downwards through $C_{free}$, until $C_{obs}$ is reached. Every intersection forms a cell, and thus the environment becomes a set of cells. All cells are triangles or trapezoids, so they are convex. Hence, if we put a roadmap node randomly inside a cell, all other configurations within this cell can be connected to the roadmap point without intersecting $C_{obs}$. It follows that connectivity of the environment is preserved through this cell decomposition. + +Now that we have different cells that compose the environment, the roadmap can be built by placing a sample point at the centroid of each cell and at midpoint of each boundary. Once the roadmap is constructed, given an initial and goal configuration $q_I$ and $q_G$, a path can always be extracted from the roadmap. To form a path from a roadmap, searching algorithms are needed, which will be introduced in section 15.5. The convex properties of the cells and placement of the sample points guarantee that if a path exists it will not intersect $C_{obs}$ (i.e. no collision will occur). + +Other ways of constructing the roadmap are based on different requirements. Figure 15.6 shows two requirements: maximum clearance and minimum distance. Specifically, if maximum clearance from $C_{obs}$ is required, we would like to place roadmap points along the middle of the tunnel that represents the boundary of $C_{obs}$. A voronoi diagram is used to compute the roadmap by connecting $q_I$ and $q_G$ along edges of the constructed diagram in polygons. In the other case where the shortest path is the goal, a visibility graph can be used. A visibility graph represents the set of unblocked lines between vertices of obstacles, $q_I$ and $q_G$. Usually shortest paths tend to graze the corners of $C_{obs}$ as much as possible. + +\begin{figure}[h] +\begin{center} +\includegraphics{fig15_6.PNG} +\caption{Other Cell Decomposition Methods Based on Requirements.} +\end{center} +\end{figure} + +\subsection{3D Vertical Decomposition} +Computing cell decomposition is much harder for higher-dimensional cases, though vertical cell decomposition is straightforward in 2D. We consider the special case where $C_{obs}$ is piecewise linear and polyhedral. + +It turns out that we can extend the 2D vertical decomposition method by applying the idea of plane-sweeping to higher dimensions. Plane-sweeping refers to sweeping a plane across the space, only to stop where critical change occurs in information. In 3D, assume a polyhedral robot can translate in $\mathbb{R}^3$, and the obstacles are polyhedral. Thus $C_{obs} \in \mathbb{R}^3$ is polyhedral as well. The 3D vertical decomposition algorithm is as follows (shown in Figure 15.7). + +Let (x, y, z) denotes a point in $\mathbb{R}^3$. Vertical decomposition yields 3-cell, 2-cell and 1-cell. A generic 3-cell is bounded by 6 planes, and its cross section for a fixed x yields a trapezoid or triangle in a plane parallel to the yz plane. Two sides of a 3-cell are parallel to the yz plane, and two other sides parallel to xz plane. The 3-cell is bounded above and below by two polygonal faces of $C_{obs}$. + +The general idea is to sweep a plane perpendicular to x axis, where each fixed value of x produces a 2D polygonal slice of $C_{obs}$. Three example slices are shown in the bottom of Figure 15.7. Each slice is parallel to yz plane and simplify the 3D problem to a problem that can be solved by 2D vertical decomposition method. The middle slice shows the condition where the sweeping plane just encounters a vertex of a convex polyhedron, represented as a dot. This corresponds to an x value in interest, as critical change +must occur in the slices. Hence, the 3D cell decomposition can be developed incrementally by sweeping through planes to update 2D vertical cell decomposition, in order to incorporate critical changes. To construct a roadmap, sample points are placed at center of each 3-cell and 2-cell. Edges are added to the roadmap by connecting each 3-cell to an adjacent 2-cell. + +\begin{figure}[H] +\begin{center} +\includegraphics{fig15_7.PNG} +\caption{3D Cell Decomposition Extended from 2D.} +\end{center} +\end{figure} + +\section{Graph-Search Algorithms} \label{Graph-Search Algorithms} +Suppose our environment map has been converted into a connectivity graph using one of the graph generation methods presented earlier. Whatever map representation we choose, the goal of path planning is to find the best path in the map’s connectivity graph between the start and the goal, where best refers to the selected optimization criteria. In this section, we present several popular search algorithms in mobile robotics. + +\subsection{Concepts in graph search} +We introduce the concepts which are all functions of the node $n$ (and an adjacent node $n'$): +\begin{itemize} + \item $g(n)$: Path cost (accumulated cost from the start node to any given node $n$). + + \item $C(n, n')$: Edge traversal cost (cost from a node $n$ to an adjacent node $n'$). + + \item $h(n)$: Heuristic cost (expected cost from a node $n$ to the goal node). + + \item$f(n)$: Expected total cost (from start to goal via state $n$). -%% --- -% ADD NOTES HERE +And $f(n) = g(n) + \epsilon h(n)$, where $\epsilon$ is a parameter that assumes algorithm-dependent values. +\end{itemize} +\subsection{Discriminator graph search} +\begin{itemize} +\item No heuristic function employed + \begin{itemize} + \item[-] Uniform traversal cost with a simpler form and obtains faster execution speeds. \\ + e.g.) depth-first, breadth-first \\ + \item[-] Nonuniform traversal cost with a higher algorithmic complexity.\\ + e.g.) Dijkstra’s algorithm + \end{itemize} + -%% --- +\item Heuristic function employed + \begin{itemize} + \item[-] Incorporates additional information about the problem set and thus often allows for faster convergence of the search query. \\ + e.g.) $e = 1$: optimal A* algorithm, $e > 1$: sub-optimal or greedy A* variant +\end{itemize} +\end{itemize} + +\subsection{Various graph search algorithms} +\subsubsection{Depth-First Search (DFS)} +Depth-first search expands each node up to the deepest level of the graph, until the node has no more successors. As those nodes are expanded, their branch is removed from the graph and the search backtracks by expanding the next neighboring node of the start node until its deepest level and so on. DFS stores only a single path from the start nodes for each node on the path, leading to a lower memory requirement. However, previously visited nodes might be visited and the algorithm might enter redundant paths. + +\begin{figure}[h!] +\begin{center} +\includegraphics[width=0.5\textwidth]{fig15_8.PNG} +\caption{Depth First Search} +\end{center} +\end{figure} + +\subsubsection{Breadth-First Search (BFS)} +Breadth-first search begins with the start node and explores all of its neighboring nodes. Then for each of these nodes, it explores all their unexplored neighbors and so on. This search always returns the path with the fewest number of edges between the start and goal node. + +\begin{figure}[h!] +\begin{center} +\includegraphics[width=0.5\textwidth]{fig15_9.PNG} +\caption{Breadth First Search} +\end{center} +\end{figure} + +\subsubsection{Dijkstra’s Algorithm (Shortest Path First)} +Dijkstra’s algorithm is similar to Breadth-first search, except that edge costs may assume any positive value and the search still guarantees solution optimality. Dijkstra’s algorithm uses the concept of the heap, a specialized tree-based data structure and selects the next $q$ as: $q = \textrm{arg} \min_{q \in Q} C(q)$ + +\subsection{Label Correcting Algorithm} +The label-correction algorithm is a general type of shortest path algorithm which includes common graph search algorithms like breadth-first search, depth-first search, A*, Dijkstra’s algorithm as special cases. +The idea is to progressively discover shorter paths from the origin to every other node $q$, and to maintain the lowest cost path (from $q_I$ to $q$) found so far in a variable $C(q)$. +\vspace{3em} +\begin{figure}[h] +\begin{center} +\includegraphics[width=0.8\textwidth]{fig15_10.PNG} +\caption{Label Correcting Algorithm} +\end{center} +\end{figure} +\newpage +\textbf{Algorithm}: +\vspace{-0.7em} +\begin{itemize} + \item \textbf{Step 1}: Remove a node q from the frontier queue and for each child q0 of q, execute step 2. + \item \textbf{Step 2}: If $ C(q) + C(q, q') \leq \min(C(q'),\textrm{UPPER})$, set $ C(q') := C(q) + C(q, q')$ and set $q$ to be the parent of $q'$. In addition, if $q' \neq q_G$, place $q'$ in frontier queue if it is not already there, while if $q' = q_G$, set UPPER to the new value $C(q) + C(q, q_G)$ + \item \textbf{Step 3}: If the frontier queue is empty, terminate, else go to step 1. + \item \textbf{Initialization}: Set the labels of all nodes to $\infty$, except the label of the origin node which is set to 0. +\end{itemize} +\vspace{-1em} + +\section{Correctness and Improvements} \label{Correctness and Improvements} +Label correcting algorithms are guaranteed to find the shortest feasible path from an initial state to a goal state, given one exists. This attribute gives rise to the Correctness Theorem. \vspace{-0.5cm} +\paragraph{Theorem} If a feasible path exists from initial state $q_I$ to goal state $q_G$, then the algorithm terminates in finite time with path cost $C(q_G)$ equal to the optimal cost of traversal, $C^*(q_G)$.\\ + +However, computing this path can be computationally intensive, especially in high-dimensional problems. This challenge arises from an explosion in the number of states expanded during the search procedure. \\ + +Consider Fig \ref{fig:graph_search}, Depth-First Search and Breadth-First Search algorithms do not greedily select states from a frontier queue for directed expansion. Hence, these algorithms search the configuration space without direction/relatively uniformly without considering the next state's cost or location relative to the goal. Dijkstra's algorithm improves on Depth-First Search and Breadth-First Search by greedily selecting states from the frontier queue based on cost and by not revisiting previously visited states. However, Dijkstra's algorithm does not take the goal state location into account, potentially leading to wasted effort. Concentrating the search in regions closer to the goal state may reduce the number of states expanded during the search, speeding up the motion planning process. + +\begin{figure}[h] +\begin{center} +%\includegraphics{fig15_11.PNG} +\begin{tikzpicture}[scale=0.9, transform shape] +\draw (0,0) circle (0.6); +% \draw (0,0) circle (1.0); +\draw (0,0) circle (1.2); +% \draw (0,0) circle (2.0); +\draw (0,0) circle (1.8); +\draw (0,0) circle (2.4); + +\filldraw[yellow] (0,0) circle (2pt); +\draw (0,0) node[anchor=west] {$s_{\text{start}}$}; +\filldraw[green] (2.4,0) circle (2pt); +\draw (2.4,0) node[anchor=west] {$s_{\text{end}}$}; + +\draw (-3,0) node[anchor=west,color=red] {Wasted Effort?}; + +\end{tikzpicture} +\caption{Uniform Graph Search Algorithm Exploration} +\label{fig:graph_search} +\end{center} +\end{figure} + +\subsection{A*: Improving Dijkstra} +Dijkstra's algorithm orders nodes added to the frontier queue (priority queue) by ``cost-to-arrival" $C(q)$. The A* algorithm (given below) improves on Dijkstra by adding a heuristic function $h(q)$ that models cost to go to the goal state from state $q$, i.e. A* orders by ``cost-to-arrival" + (approximate) ``cost-to-go." This heuristic function, for example distance to goal, guides exploration towards the region of the configuration space close to the goal, leading to faster computations as less states are expanded. Thus, the test +condition changes from +\[ C(q) + C(q, q') \leq \text{UPPER} \qquad +\text{to} \qquad C(q) + C(q, q') + h(q') \leq \text{UPPER}. \] +The heuristic must be a \textit{positive underestimate} of the true cost to the goal. This modification of the test condition reduces the number of nodes placed in the frontier queue and still guarantees that an optimal path will be returned. In practice, it leads to much faster runtimes.\\ + +\SetKwInput{KwReturn}{Return} +\begin{algorithm}[H] +\caption{A* Algorithm} + \KwData{$q_{\text{init}}$, $q_{\text{goal}}$} + \KwResult{path} + $C(q) = \infty, \;\; f(q) = \infty\;\; \forall q$\; + $C(q_{\text{init}}) = 0$, $f(q_{\text{init}}) = h(q_{\text{goal}})$\; + \tcp{Init frontier queue and explored vertices} + OPEN = $\{q_{\text{init}}\}$, CLOSED = $\{\}$ \; + \While{OPEN is not empty}{ + $q = \argmin_{q' \in \text{OPEN}}f(q')$\; + \If{$q$ == $q_{\text{goal}}$}{ + \KwReturn{path} + } + OPEN.remove($q$); CLOSE.add($q$)\; + \For(\tcp*[f]{Exploration}){$q'$ in \{$q'$ | $(q,q')$ in G, $q'$ not in CLOSED\}}{ + OPEN.add($q'$)\; + \If{$C(q') < C(q) + C(q,q')$}{ + continue;} + $q'$.parent = $q$; $C(q') = C(q) + C(q,q')$\; + $f(q') = C(q') + h(q')$ + } + } + \KwReturn{failure} +\end{algorithm} + + +Figure \ref{fig:astar_example} illustrates A* implemented on a grid. Black states represent obstacles. Yellow states represent nodes in the frontier queue. Blue states represent explored nodes. Green states indicate the final path. The state with the minimum total estimated cost (including the heuristics cost) is selected from the frontier queue for expansion. + + +\newcolumntype{C}[1]{% + >{\vbox to 5ex\bgroup\vfill\centering}% + m{#1}% + <{\egroup}} + +\newcommand{\stategh}[2]{ +\vspace{0.1cm}c={#1}\\ \vspace{0.3cm}h={#2} +} + +\newcommand{\stateghblank}[2]{ +} + +\newcommand{\stateghopen}[2]{ +\cellcolor{blue}\vspace{0.1cm}c={#1}\\ \vspace{0.3cm}h={#2} +} + +\newcommand{\stateghpq}[2]{ +\cellcolor{yellow}\vspace{0.1cm}c={#1}\\ \vspace{0.3cm}h={#2} +} + +\newcommand{\stateghpath}[2]{ +\cellcolor{green}\vspace{0.1cm}c={#1}\\ \vspace{0.3cm}h={#2} +} + +\begin{figure}[h!] +\begin{center} +% \includegraphics[scale=0.90]{fig15_12.PNG} +\resizebox{.24\columnwidth}{!}{ +\begin{tabular}{|C{1cm}|C{1cm}|C{1cm}|C{1cm}|} + \hline + goal & \cellcolor{black} & \stateghpq{1.4}{2.0} & \stateghpq{1.0}{3.0} \tabularnewline + \hline + \stateghblank{3.8}{1.0} & \cellcolor{black}& \cellcolor{black} & \cellcolor{blue}{start} \tabularnewline + \hline + \stateghblank{3.4}{2.0} & \stateghblank{2.4}{2.4} & \stateghpq{1.4}{2.8} & \stateghpq{1.0}{3.8} \tabularnewline + \hline + \stateghblank{3.8}{3.0} & \stateghblank{2.8}{3.4} & \stateghblank{2.4}{3.8} & \stateghblank{2.8}{4.2}\tabularnewline + \hline +\end{tabular} +} +\resizebox{.24\columnwidth}{!}{ +\begin{tabular}{|C{1cm}|C{1cm}|C{1cm}|C{1cm}|} + \hline + goal & \cellcolor{black} & \stateghopen{1.4}{2.0} & \stategh{1.0}{3.0} \tabularnewline + \hline + \stateghblank{3.8}{1.0} & \cellcolor{black}& \cellcolor{black} & \cellcolor{blue}{start} \tabularnewline + \hline + \stateghblank{3.4}{2.0} & \stateghblank{2.4}{2.4} & \stategh{1.4}{2.8} & \stategh{1.0}{3.8} \tabularnewline + \hline + \stateghblank{3.8}{3.0} & \stateghblank{2.8}{3.4} & \stateghblank{2.4}{3.8} & \stateghblank{2.8}{4.2}\tabularnewline + \hline +\end{tabular} +} +\resizebox{.24\columnwidth}{!}{ +\begin{tabular}{|C{1cm}|C{1cm}|C{1cm}|C{1cm}|} + \hline + goal & \cellcolor{black} & \stateghopen{1.4}{2.0} & \stateghopen{1.0}{3.0} \tabularnewline + \hline + \stateghblank{3.8}{1.0} & \cellcolor{black}& \cellcolor{black} & \cellcolor{blue}{start} \tabularnewline + \hline + \stateghblank{3.4}{2.0} & \stateghblank{2.4}{2.4} & \stategh{1.4}{2.8} & \stategh{1.0}{3.8} \tabularnewline + \hline + \stateghblank{3.8}{3.0} & \stateghblank{2.8}{3.4} & \stateghblank{2.4}{3.8} & \stateghblank{2.8}{4.2}\tabularnewline + \hline +\end{tabular} +} +\vspace{0.3cm} +\resizebox{.24\columnwidth}{!}{ +\begin{tabular}{|C{1cm}|C{1cm}|C{1cm}|C{1cm}|} + \hline + goal & \cellcolor{black} & \stateghopen{1.4}{2.0} & \stateghopen{1.0}{3.0} \tabularnewline + \hline + \stateghblank{3.8}{1.0} & \cellcolor{black}& \cellcolor{black} & \cellcolor{blue}{start} \tabularnewline + \hline + \stateghblank{3.4}{2.0} & \stateghpq{2.4}{2.4} & \stateghopen{1.4}{2.8} & \stategh{1.0}{3.8} \tabularnewline + \hline + \stateghblank{3.8}{3.0} & \stateghpq{2.8}{3.4} & \stateghpq{2.4}{3.8} & \stateghpq{2.8}{4.2}\tabularnewline + \hline +\end{tabular} +}\\ +\resizebox{.24\columnwidth}{!}{ +\begin{tabular}{|C{1cm}|C{1cm}|C{1cm}|C{1cm}|} + \hline + goal & \cellcolor{black} & \stateghopen{1.4}{2.0} & \stateghopen{1.0}{3.0} \tabularnewline + \hline + \stateghpq{3.8}{1.0} & \cellcolor{black}& \cellcolor{black} & \cellcolor{blue}{start} \tabularnewline + \hline + \stateghpq{3.4}{2.0} & \stateghopen{2.4}{2.4} & \stateghopen{1.4}{2.8} & \stategh{1.0}{3.8} \tabularnewline + \hline + \stateghpq{3.8}{3.0} & \stategh{2.8}{3.4} & \stategh{2.4}{3.8} & \stategh{2.8}{4.2}\tabularnewline + \hline +\end{tabular} +} +\resizebox{.24\columnwidth}{!}{ +\begin{tabular}{|C{1cm}|C{1cm}|C{1cm}|C{1cm}|} + \hline + \cellcolor{yellow}goal & \cellcolor{black} & \stateghopen{1.4}{2.0} & \stateghopen{1.0}{3.0} \tabularnewline + \hline + \stateghopen{3.8}{1.0} & \cellcolor{black}& \cellcolor{black} & \cellcolor{blue}{start} \tabularnewline + \hline + \stategh{3.4}{2.0} & \stateghopen{2.4}{2.4} & \stateghopen{1.4}{2.8} & \stategh{1.0}{3.8} \tabularnewline + \hline + \stategh{3.8}{3.0} & \stategh{2.8}{3.4} & \stategh{2.4}{3.8} & \stategh{2.8}{4.2}\tabularnewline + \hline +\end{tabular} +} +\vspace{0.3cm} +\resizebox{.24\columnwidth}{!}{ +\begin{tabular}{|C{1cm}|C{1cm}|C{1cm}|C{1cm}|} + \hline + \cellcolor{blue}goal & \cellcolor{black} & \stateghopen{1.4}{2.0} & \stateghopen{1.0}{3.0} \tabularnewline + \hline + \stateghopen{3.8}{1.0} & \cellcolor{black}& \cellcolor{black} & \cellcolor{blue}{start} \tabularnewline + \hline + \stategh{3.4}{2.0} & \stateghopen{2.4}{2.4} & \stateghopen{1.4}{2.8} & \stategh{1.0}{3.8} \tabularnewline + \hline + \stategh{3.8}{3.0} & \stategh{2.8}{3.4} & \stategh{2.4}{3.8} & \stategh{2.8}{4.2}\tabularnewline + \hline +\end{tabular} +} +\resizebox{.24\columnwidth}{!}{ +\begin{tabular}{|C{1cm}|C{1cm}|C{1cm}|C{1cm}|} + \hline + \cellcolor{green}goal & \cellcolor{black} & \stateghopen{1.4}{2.0} & \stateghopen{1.0}{3.0} \tabularnewline + \hline + \stateghpath{3.8}{1.0} & \cellcolor{black}& \cellcolor{black} & \cellcolor{green}{start} \tabularnewline + \hline + \stategh{3.4}{2.0} & \stateghpath{2.4}{2.4} & \stateghpath{1.4}{2.8} & \stategh{1.0}{3.8} \tabularnewline + \hline + \stategh{3.8}{3.0} & \stategh{2.8}{3.4} & \stategh{2.4}{3.8} & \stategh{2.8}{4.2}\tabularnewline + \hline +\end{tabular} +} +\caption{A* Algorithm Example} +\label{fig:astar_example} +\end{center} +\end{figure} + + +\subsection{Benefits and Drawbacks of Graph-based Approaches} +The graph-based approaches are simple, easy to use, and fast for some problems. However, this approach presents two drawbacks. +\begin{enumerate} +\item These algorithms are resolution dependent, and this resolution is decided a priori. We might find that this resolution is not fine enough to uncover the best path or any path at all for the \textit{original} motion planning problem (for example, if a narrow passage is missed by the discretization). +\item These approaches don't scale well with robot degrees of freedom. Usually, we need to construct a grid with number of dimensions equal to the number of degrees of freedom of our robot. Thus, the number of gird cells increase exponentially, which also increases the number of neighbors of each vertex exponentially in the number of degrees of freedom. Thus, the graph-based approaches become less effective for high dimensional problems (more than 3 DOFs). +\end{enumerate} + +\subsection{Further Reading} + +\textbf{Motion Planning Resources:} +H. Choset, W. Burgard, S. Hutchinson, G. Kantor, L. E. Kavraki, K. Lynch, and S. Thrun, Principles of Robot Motion: Theory, Algorithms, and Implementation, MIT Press, April 2005. + +Jean-Claude Latombe, Robot Motion Planning, 1991, Kluwer Academic Publishers + +Mark de Berg; Marc van Kreveld; Mark Overmars & Otfried Schwarzkopf (2000). Computational Geometry (2nd revised ed.). Springer-Verlag. ISBN 978-3-540-65620-3. Chapter 13: Robot Motion Planning: pp. 267–290. + +\textbf{Graph Search Algorithm Resources:} + +Knuth, D.E. (1977). "A Generalization of Dijkstra's Algorithm". Information Processing Letters. 6 (1): 1–5 + +Hart, P. E.; Nilsson, N. J.; Raphael, B. (1972). "Correction to "A Formal Basis for the Heuristic Determination of Minimum Cost Paths"". SIGART Newsletter. 37: 28–29 \printbibliography \subsubsection*{Contributors} -Winter 2019: [Your Names Here] \\ +Winter 2019: Warren Cheng, Junwu Zhang, Theo Diamandis, Jeremy Crowley, Laura Lee, Kaushik Ram Sadagopan\\ Winter 2018: Ryan Richey, Hyoungju Seo, Ola Shorinwa, Josh Sullivan, Danning Sun \end{document} +