[AI614] 02. Introduction to Motion planning

학교 수업을 듣고 복습차 lec 5-7까지 정리한 review 글입니다.

Outline: What does this review contain?

1. Problem statement
2. Discretization-based methods -A*
3. Sampling-based algorithms -RRTs, PRMs
4. Probabilistic completeness -RRT*

Basic notion

• Configuration: A specification of the position of all points of a robot 로봇 모든 부위 위치 -센터 위치인데 shape알면 다 되는건가? robot shape랑 robot configuration이랑 무슨 차이지?
  • If we know the joint angles (ex. $(\alpha, \beta)$), then we know the position of every point of the robots.
• Configuration space: A space of configuration 이게 어떻게 space로 존재하지? 갈수있는 곳을 표시하는 건가? Configuration (free) space? Geometry of the environment랑은 어떤 차이지? Given으로 안 주어져있잖아..
• Heuristic function
  • Kind of state-value function. cost (현재 state --> Goal)
  • This function takes the state as an input and returns a sum of costs to a goal. It is denotes as $h(s)$, formally defined as $h(s_t) = \sum_{t'=t+1} cost(s_t)$
  • The heuristic function depends on what future path you take.
• Completeness
  • Completeness on discrete: A planning algorithm is complete if it returns a solution when there is one, and return none if it does not exist. Related to dilemma on exploitation vs. exploration.
  • Probabilistic completeness: Completeness on continuous search problems. As the number of iterations reaches infinity, the probability of finding a solution reaches 1

⛵ Problem statement

• Given
  • Geometry of the environment Geometry는 대부분 어떤 정보를 나타내지?
  • Robot shape
  • Initial configuration (처음 로봇 위치) $q_0$
  • Goal configuration (로봇 골 위치)$q_g$
• Unknown: Fast and optimal algorithm 

⛵ Discretization-based methods -A*

1. C-space discretization and graph construction
2. Graph search: Use of a search tree over the state space in order to find a goal.
   1. Depth First Search (DFS) --> Slow and sub-optimal
      • Expands the deepest node first
   2. Dijkstra's algorithm --> Optimal but not necessarily faster
      
      • Assume we are additionally given a cost function.
      • Expands the lowest-cost node first
   3. A*  --> Optimal and faster
      • Assume we are additionally given a heuristic function. What is the difference between cost function? 우리가 일반적으로 아는 heuristic과 동일한 개념인가? cost는 시작점-기존까지의 cost인데 heuristic은 기존-끝점에서의 성격인 것 같다. 맞나? 아 그리고 이런 heuristic은 real-world에서 주어지지 않지 않나?
      • Expand the frontier with the minimum cost + heuristic $\min_{n \in F} g(n) + h(n,s)$
      • A* is acceptable if it uses an acceptable heuristic function

Challenges to discretization-based methods

1. Obstacle construction in c-space is non-trivial, because of the complex robot shape.
2. Constructing c-space is expensive in high-dimensional spaces.

⛵ Sampling-based algorithms -RRTs, PRMs

To deal with the fact that we have a continuum of choices, we explore by sampling few choices. The sampling-based motion planners' high-level ideas are as follows:

1. Create a data structure of paths by:
   1. Random sampling of a new configuration
   2. Connect the new configuration to the data structure of paths.
2. Repeat until we find a path from point A to point B

RRT and PRM are the most common algorithms. They build a set of feasible trajectories using a tree rooted at the initial configuration

RRTs: Rapidly exploring Random Trees

• Input
  • $q_0, q_g$
  • Collision detector $D:Q \leftarrow \{ 0,1 \}$ Common collision detector는 이렇게 표현되는가? 위에 것들은 collision detector가 왜 필요없지?
  • Metric $\phi:Q \times Q \leftarrow \mathcal{R}$
• Process
  1. Begin with the initial configuration $T \leftarrow \{ q_0 \}$
  2. Randomly sample a new free configuration $q_{rand} \leftarrow \textrm{SampleFree}$
  3. Find the closest configuration in the tree $q_{closest} \leftarrow \textrm{Nearest} (T, q_{rand})$
  4. Extend $Q_{new} \leftarrow \textrm{Extend}(q_{closest}, q_{rand})$
  5. Add $T.\textrm{add_node}(\{Q_{new}\})$,  $T.\textrm{add_edge}(\{Q_{new}\})$

Probabilistic completeness on RRTs: Bidirectional RRTs

Balancing exploration vs. exploitation on RRTs

1. To balance exploration, we sample with probability $p$. With probability $p$, exploitation. Otherwise, exploration. 
2. To boost up the sampling configuration to search faster, we use a bidirectional approach. You sample a new configuration and extend towards the sampled configuration from the closest node from each tree.

PRMs: Probabilistic Roadmaps

PRMs address challenges we need to rebuild tree when we have a different problem instance in the same environments (i.e. changing the initial or goal configs, or both). PRMs capture the connectivity in configuration space by a network of paths.

It has two phases:

1. Construction phase: Constructing the roadmap
   • Capture the connectivity in c-space by using random samples
2. Query phase: Querying for a path using the roadmap
   • Finds a path from initial to goal configuration using the constructed roadmap

Weakness of sampling-based algorithms

For narrow passages, that probability would be very small. It takes a lot of iterations to find a solution.

⛵ Optimal sampling-based motion planning -RRT*