[RRT] Informed RRT* , Gammell

Informed RRT*: Optimal Sampling-based Path Planning Focused via Direct Sampling of an Admissible Ellipsoidal Heuristic

[1404.2334] Informed RRT*: Optimal Sampling-based Path Planning Focused via Direct Sampling of an Admissible Ellipsoidal Heuristic

Informed RRT*: Optimal Sampling-based Path Planning Focused via Direct Sampling of an Admissible Ellipsoidal Heuristic

 

0. abstract

• RRT는 single-query problem에 대해서 효율적으로 solution을 찾는다. 이러한 이유로 motion planning에 널리 사용된다. 
  RRT*는 RRT를 확장해서 문제의 최적의 solution을 찾는다. 이 과정에서 planning domain의 every state에 대해서 initial state로부터의 optimal path를 점진적으로 찾는 방식으로 작동한다.
  
  • 위와 같은 동작은 비효율적이며, RRT의 single-query 특성과도 일치하지 않는다.
• path length를 minimize 하려는 문제에서는 해를 개선할 수 있는 상태의 부분집합( subset of states )이 '장축이 긴 초타원체'로 묘사할 수 있다.
  이 부분 집합( subset )을 직접 샘플링하지 않으면, 해결책을 개선할 확률은 세계가 커지거나 상태 차원이 높아질수록 의로 작아진다.
  
  • 본 논문에서는 이 subset을 직접 sampling함으로써, 탐색을 집중시키는 방법을 제안한다.
• 제안된 sampling 기법의 장점은 새로운 알고리즘은 Informed RRT를 통해서 입증된다.
  • 이 방법은 RRT와 동일한 probabilistic completeness와 최적성을 유지하면서, 수렴속도와 최종 해의 품질을 향상시킨다.
    본 알고리즘에서 RRT*의 간단한 수정 형태도 제시되며, 향후에는 더 고급 path planning알고리즘에 의해 확장될 수 있다.
  • 실험을 통해서, Informed RRT가 수렴 속도, 최종 해 비용 , 어려운 통로 탐색 능력에서 RRT보다 우수함을 보여주며, state dimension 및 planning problem range에 덜 의존함을 입증했다. 

---

1. Introduction

• motion planning problem은 일반적으로 continuous state space를 먼저 discretization함으로써 해결되며, 이산화( discretization )방법은 그래프 기반 탐색을 위한 grid를 사용하거나, stochastic incremental 탐색을 위한 random sampling 방식중 하나이다.
  • A*와 같은 그래프 기반 탐색 알고리즘은 종종 resolution complete와 resolution optimal을 만족한다.
    • 즉, 해가 존재할 경우에 optimal solution을 찾고, 존재하지 않는다면 실패를 반환한다는 것이 보장된다. 그러나 이러한 그래프 기반의 알고리즘은 state dimension이나 problem range가 커질수록 확장성이 떨어진다.
• RRT , PRM , EST같은 확률기반 탐색 알고리즘들은, state space를 이산화하지 않고 sampling을 하는 방법을 사용한다. 이는 문제의 크기에 따라서 더 효율적으로 확장할 수 있게하며, 운동역학 제약도 직접 고려할 수 있게 해준다.
  • 하지만 이러한 방법은 완전성 보장이 약해지며, RRT는 확률적으로 완전하다. ( 이는 반복횟수가 무한에 가까워질수록 해를 찾을 확률이 1에 수렴한다는 것을 의미 )
  • 최근까지 이러한 샘플링 기반의 알고리즘은 해의 최적성에 대해서 보장하지 않았다. 
    • Urmson과 Simmons는 휴리스틱을 사용해 샘플링을 편향시키면 RRT 해가 개선된다는 것을 발견했지만, 그 효과를 수학적으로 정량화하지는 않았다.
    • Ferguson과 Stentz는 해의 길이가 가능한 개선 범위를 상한으로 제한한다는 점을 인식하고, 점진적으로 더 작은 문제들을 해결하는 반복적인 anytime RRT 기법을 제안하였다.
    • Karaman과 Frazzoli는 RRT가 거의 확실히(suboptimal with probability one) 비최적 경로를 반환한다는 것을 보였으며, 모든 RRT 기반 기법은 거의 확실히 비최적이라는 점을 입증하였다. 그들은 RRT와 PRM의 최적 변형으로 각각 RRT*와 PRM*이라는 새로운 최적 플래너 계열을 제안하였다. 이 알고리즘들은 **점근적으로 최적(asymptotically optimal)**이며, 반복 횟수가 무한에 가까워질수록 최적 해를 찾을 확률이 1에 가까워진다.
      • RRT는 기존 상태 그래프가 향후 확장을 편향시키기 때문에 점근적으로 최적이지 않다.
        RRT*는 그래프를 점진적으로 재구성(rewiring)함으로써 이를 극복한다.
        새롭게 추가되는 상태는 단순히 트리에 추가될 뿐만 아니라, 트리 내 인접 상태들의 부모 후보로도 고려된다. 전역적으로 균일 샘플링을 수행하면, 이는 문제 도메인의 모든 상태에 대해 최적 경로를 찾는 방식으로 작동하게 되고, 결국 전체 최적 해를 점근적으로 찾는 알고리즘이 된다.
        그러나 이는 RRT의 single-query 특성과 일치하지 않으며, 고차원 상태 공간에서는 계산 비용이 매우 커진다.
• 본 논문에서는 n차원에서 경로의 길이를 최소화를 중심으로 하는 focused optimal planning문제를 다룬다.
  • 이러한 문제에서는 해를 개선하려면 타원형 부분집합(ellipsoidal subset)에 속하는 state를 추가해야 한다는 것이 필요조건이다.
  • 우리는 전역 균일 샘플링을 통해 이러한 state들이 추가될 확률이 계획 문제의 크기가 커지거나 해가 이론적 최소값에 접근할수록 임의의 작은 값으로 수렴함을 보이고, 이 타원체 하위 집합을 직접 샘플링하는 정확한 방법을 제시합니다.
    • 또한, 엄격한 가정(즉, 장애물이 없는 경우) 하에서 이러한 직접 샘플링이 최적 해로의 선형 수렴(linear convergence)을 가져온다는 것을 보입니다.

fig1. 무작위 환경에서 RRT와 Informed RRT가 찾은 동등한 비용의 경로들.

• Informed RRT*는 첫 번째 해가 발견되기 전까지는 RRT*처럼 작동하며, 첫 해가 발견된 이후에는 해를 개선할 가능성이 있는 상태 부분집합만 샘플링한다. 이 부분집합은 허용 가능한 휴리스틱(admissible heuristic)을 통해 정의되며, 탐색과 활용 사이의 균형을 암묵적으로 조절하고 추가적인 튜닝 파라미터나 가정이 필요 없다.
  • 물론 휴리스틱이 항상 탐색을 개선하는 것은 아니지만, 실제 플래닝 상황에서는 매우 실용적이다. 만약 휴리스틱이 추가적인 정보를 제공하지 못허면 Informed RRT*는 RRT*와 동일하게 동작한다.
• Informed RRT*는 RRT*에 간단한 수정을 가한 것이지만, 명확한 개선 효과를 보여준다. 단순한 구성에서는 기존 RRT*와 동일한 성능을 보이며, 복잡한 구성에서는 수십 배 빠른 성능 향상을 보인다(Fig. 2 참고 )
  • 집중된 탐색(focused search) 덕분에, 이 알고리즘은 상태 차원이나 문제 범위에 대한 의존도가 적고, 더 나은 위상적으로 구별되는 경로를 더 빨리 찾을 수 있다.
  • 또한 RRT*와 동일한 계산 비용으로 더 정밀한 최적 경로를 찾을 수 있으며, 장애물이 없는 경우에는 유한 시간 내에 기계 정밀도 수준(machine zero)의 최적해를 찾을 수 있다(Fig. 3 참고).
  • 이 알고리즘은 경로 스무딩(path smoothing) 같은 다른 기법들과도 결합하여 탐색 공간을 더욱 줄이는 데 사용될 수 있다.

fig2. 무작위 환경에서 RRT와 Informed RRT의 해 비용과 계산 시간의 비교. 두 플래너 모두 같은 비용의 해를 찾을 때까지 실행되었다. a와 c는 최종결과를 보여주며, b는 해 비용과 계산 시간 간의 관계를 나타낸다. 그림 a에서도 알수 있듯이 rrt는 현재 해를 개선할수 없는 영역까지 탐색하느라 많은 자원을 소비하고 있다.

fig3. 해가 점점 개선됨에 따라서, 샘플링 영역의 크기 역시 작아지며, 이는 피드백 효과를 만들어서 최적 이론값에 가까운 결과를 만들어낸다.

 

 

• 논문의 나머지 구성은 다음과 같다.
  • 2장 : focused optimal path planning problem의 형식적 정의와 기존의 연구들을 다룬다.
  • 3장 : n차원에서 path의 length를 최소화하는 문제에서, 해를 개선할 수 있는 상태의 subset을 닫힌 형태로 추정하고, 이것이 RRT*종류의 알고리즘에 미치는 영향을 분석한다.
  • 4장 : subset을 직접 sampling하는 방법을 제시한다.
  • 5장 : informed RRT*알고리즘에 대한 소개
  • 6장 : 다양한 크기와 구성의 간단한 문제와 다양한 차원의 무작위 문제에 대해 RRT*와 Informed RRT*를 비교한 시뮬레이션 결과를 제시한다.
  • 7장 : 이 기법에 대한 논의 및 관련된 후속 연구들을 소개하며 논문을 마무리한다.

 

---

2. Background - 문제 정의 

• 본 논문에서는 RRT*와 유사하게 optimal path planning문제를 정의한다.

문제에 대한 정의 1 ( R >= 0은 0이상의 실수 집합이다. )

최적 경로를 찾는 식의 표현

문제 정의 2

• 그러나 일반적으로 f(⋅)는 알려져 있지 않기 때문에, 이를 근사하기 위해 휴리스틱 함수 f^(⋅)를 사용할 수 있다.
  이 휴리스틱이 실제 경로 비용을 절대 초과하지 않는다면, 허용 가능한 휴리스틱(admissible heuristic)이라 한다.
  • 허용 가능한 휴리스틱을 사용할 경우, 추정된 집합은 실제 집합을 항상 포함하게 되므로 X^f⊃Xf가 되고, 따라서 추정 집합에 속하는 것은 해를 개선하기 위한 필요 조건이 된다.

허용 가능한 휴리스틱

 

---

2. Background - 선행 연구 

• 1. Sample Biasing
  • 샘플 편향은 X에서 샘플을 추출할때, X f에서 더 자주 sampling되도록 확률 분포를 조정하는 방식이다.
    • 하지만 이러한 방식은 해를 개선할수 없는 states를 포함하며, 샘플 분포의 밀도가 균일하지 않게 되어 RRT*의 전제 조건을 위반한다.
  • 1-a. Heuristic - biased sampling
    • Urmson과 Simmons는 휴리스틱 값에 반비례하는 확률로 상태를 선택하는 'hRRT(Heuristically Guided RRT)'를 제안하였다. 이는 RRT보다 나은 해를 찾았지만, RRT 기반이므로 여전히 거의 확실히 비최적임을 보였다 .
    • Kiesel은 두 단계로 이루어진 f-편향(f-biasing) 기법을 사용하였다.
      먼저 문제를 추상화해 각 상태에 휴리스틱 비용을 할당한 후, 이를 기반으로 상태를 선택하고 연속적인 샘플링을 수행하였다.
      추상 상태를 기반으로 선택 확률이 결정되며, 모든 상태가 선택될 확률이 0이 아닌 값을 유지하게 설계되어 있다.
  • 1-b. path biasing
    • 현재 경로 근처에서 샘플링을 수행하는 방식이다.
      이는 현재 경로가 최적 해와 동일한 호모토피(homotopy) 클래스에 있다고 가정하지만, 이 가정은 일반적으로 성립하지 않으므로 전역 샘플링도 병행해야 한다.
    • Alterovitz은 RRM(Rapidly-exploring Roadmap)을 제안했으며, 초기 해가 발견된 후 경로 주변을 선택해 그래프를 확장하였다.
    • Akgun과 Stilman은 이중 트리 기반 RRT*에서 경로의 상태를 선택하고 그 상태의 보로노이 영역에서 샘플링하여 현재 해를 개선하였다.
    • Nasir은 *RRT-Smart에서 경로 스무딩을 적용한 뒤 해당 경로 상태들을 편향점으로 활용하였다.
    • Kim은 가시성 분석을 통해 초기 편향을 생성하는 Cloud RRT 알고리즘을 제안하였다.
• 2. Hueristic - based sample rejection
  • 이 방법은 큰 분포에서 샘플을 추출한 후, 휴리스틱 값을 기준으로 유지하거나 버리는 방식이다.
  • Akgun과 Stilman도 이 기법을 사용하였다. 반복마다 계산량은 적지만, Xf가 작아지면 유효 샘플을 찾기까지 반복 횟수가 급격히 늘어난다.
    Otte와 Correll은 C-FOREST에서 경계 상자(hyperrectangle) 기반 영역에서 샘플링하여 성능을 개선했으나, 차원이 높아질수록 성능이 감소하였다.
• 3. Graph pruning
  • RRT* 그래프 내에서 현재 해보다 비용이 높은 상태를 주기적으로 제거하여 탐색을 Xf​에 집중시키는 방식이다.
    • 그러나 샘플 추가에는 최근접 이웃 탐색이 필요하므로 단순 샘플 거절보다 계산량이 크며, 확률적 한계도 여전히 존재한다.
  • Karaman은 반복 실행 도중 해를 개선하는 anytime RRT*에서 그래프 가지치기를 적용하였다.
    • 그러나 이 방법은 inadmissible한 휴리스틱을 사용해 비용을 과대 추정하는 경우가 있어 잘못된 상태 제거가 발생할 수 있다.
  • Jordan과 Perez은 유사한 비양립 휴리스틱을 사용하였다.
  • Arslan과 Tsiotras은 RRT# 알고리즘에서 LPA*(Lifelong Planning A*) 기법을 사용해 그래프를 정교하게 관리하였다.
• 4. Anytime RRTs
  • Ferguson과 Stentz는 해가 제공할 수 있는 개선 가능 상태들을 상한으로 제한한다는 점에 주목했다.
    • Anytime RRT는 반복적으로 더 좁은 계획 영역에서 탐색을 수행하여 해를 점진적으로 개선한다.
    • 하지만 이때 기존 Xf 상태들을 버려야 한다는 단점이 있다.
• ===> Informed RRT는 위와 같은 방식들과 달리 Xf​를 명시적으로 정의하고 직접 샘플링한다.
  경로 편향 방식과는 달리 최적 해의 호모토피 클래스를 가정하지 않고, 휴리스틱 편향 방식처럼 개선할 수 없는 상태를 탐색하지도 않는다.
  • RRT 기반이므로 발견된 Xf​ 상태들을 계속 유지할 수 있으며, 샘플링 영역의 크기와 무관하게 항상 개선 가능성을 갖는 상태를 샘플링하게 된다.
    따라서 문제 크기나 현재 해의 비용에 덜 민감하게 작동하며, 휴리스틱이 유효한 정보를 제공하지 않을 경우에는 RRT*와 동일하게 작동한다.

 

---

 

fig4. 장축 타원체에 대한 일반적인 식. start와 goal은 초점( focal point)이며, 장축의 길이는 c best이고, 단축은 ( (c best)^2 - ( c min )^2 ) ^ 1/2이다.

3. ANALYSIS OF THE ELLIPSOIDAL INFORMED SUBSET

• 양의 비용 함수가 주어졌을때, state x ∈ X를 지나도록 제약된, start에서 goal까지의 최적 경로 비용 f(x)는, start에서 x까지의 최적 경로 비 g(x)와 x에서 goal까지의 최적 경로 비용 h(x)의 합과 같다. ( 즉, f = g + h )
• RRT*기반 알고리즘들은 모든 상태로 가는 최적 경로를 점근적으로 수렴하기에, 상태를 통한 해의 비용에 대한 휴리스틱 추정치인 f hat ( x )는 두 항 g(x)와 h(x)를 모두 추정해야 한다.
• 허용 가능성을 만족하는 충분 조건은, 두 구성요소 g hat (x)와 h hat (x)가 각각 g(x),h(x)에 대해 허용 가능해야 한다는 것이다.
  • ( 허용 가능성 admissibillity : 절대로 실제 최적 비용을 초과하지 않는 휴리스틱 함수. )
• n차원에서 경로의 길이를 최소화하는 문제의 경우, 유클리드 거리는 두 항 모두에 대해서 허용 가능한 휴리스틱이다.
• 해를 개선할 수 있는 상태의 정보 부분집합은 현재 해의 비용 c best를 기준으로 닫힌 형식으로 아래와 같이 표현한다.

해를 개선할 수 있는 상태의 정보 부분집합의 표현

• 위의 식은 n차원 "장축 타원체"의 일반적인 식이며, focal point는 start와 goal이며, 장축의 길이는 c best이며, 단축은 (c best)^2 - (c min )^2이다. ( fig 4참고 )
• 논문에 더 있지만 생략...

 

 

---

4. DIRECTSAMPLINGOFANELLIPSOIDALSUBSET

 

---

5. Informed RRT*

• informed rrt* 알고리즘은 알고리즘1,2,에 제시되어있다.
• 이는 RRT*와 동일하지만 3,6,7,30,31번째 줄이 추가되었다.
• RRT*처럼 state space에서 트리 T = ( V , E )를 점진적으로 구축하며, 주어진 planning문제에 대한 최적 경로를 탐색한다.
  • 여기에서 V ⊂ X free는 정점의 집합이며, E ⊂ ((X free) X (X free))는 간선의 집합이다.
  • 새로운 vertex는 free space에서 읨의로 선택된 state를 향해 그래프를 확장함으로써 추가된다. ( 새로운 vertex추가할때 마다 , 주변 vertex의 cost가 minimize되게 그래프를 rewire한다. )
• RRT*와는 다르게, 한번의 solution이 발견되면, 해당 솔루션을 개선할 수 있는 planning문제의 일부 영역에 집중한다. 
  • 이는 타원형 휴리스틱( ellipsoidal heuristic )을 직접 샘플링함으로써 이루어진다.
  • solution이 발견될때마다( 30번째 라인), Informed RRT*는 그것들을 가능한 solution 목록에 추가하고( 31번째 라인), 그 목록의 최소값( 6번째 라인 )을 사용하여, 개선 가능 영역 X f를 계산하고 샘플링한다. (7번째 라인 )
    • 관례적으로, 빈 집합의 최솟값을 무한대로 간주한다.

 

• 아래는 RRT*에는 없는 서브 함수들에 대한 설명이다.
  • Sample : free space에 존재하는 두개의 pose가 주어진다.( start와 goal같이... )
    • 함수는 전체 space에 존재하는 x new를 독립적이고 동일하게 분포된 샘플로 반환한다. ( 단, x from과 x to 사이의 최적 경로가 x new를 통과하도록 제한할때, 해당되는 비용이 c max보다 작아야한다. )
    • 대부분의 planning문제에서는 from=start , to=goal이며, 알고리즘 2의 2~4번째 줄은 문제 시작시 한번만 계산하면 됨. )
  • SampleUitNBall : 원점을 중심으로 하는 단위 n-구의 부피 내에서 균일한 샘플을 반환한다.
    • 즉, x ball ​∼ U( X ball ​)
  • RotationToWorldFrame : 하이퍼타원의 두 초점으로 주어진 두 pose인 x from과 x to이 주어진다.
    • 이 함수는 하이퍼 타원 기준 프레임에서 월드 프레임으로의 회전 행렬 C ∈ SO ( n )을 반환하며, 대부분의 planning문제에서는 이 회전 행렬을 문제 시작시에 1번만 계산하면 된다.
  • InGoalRegion : free space에 존재하는 새로운 psoe가 주어질때, 이 함수는 planning 문제에서 정의된 목표 영역인 X goal안에 존재하면 True , 그렇지 않다면 False를 반환한다. 
    • 일반적으로 X goal은 목표인 x goal을 중심으로한 반지름 r goal의 구로 표현된다.

 

 

• Rewiring 반경 계산
  • 각 반복마다, 재연결 반경인 rRRT는 아래의 조건을 만족해야 한다.
    • 1. 최적성에 거의 확실하게 수렴하도록 충분히 커야한다.
    • 2. 동시에 후보점들의 수를 계산 가능할 만큼 작게 유지해야한다.

 

• Informed RRT*는 기존과는 다르게, 해에 대한 solution을 개선할수 있는 subset에서 균일하게 샘플링한다. 그러니 rewirimg의 반경도 해당 되는 subset의 측도와 내부의 정점들을 기반으로 계산할 수 있다.
  • 이로 인해서 필요없는 rewiring의 횟수가 줄어들며 성능을 향상 시킨다.

---

 

fig5

fig 6 , 7

6. SIMULATIONS

• informed RRT는 RRT*와 다양한 단순 계획문제들에서 비교되었다.( fig 1, 2 , 5 , 6 ,7 )
  • 5,6,7은 단순한 문제이며, 특정한 도전과제를 테스트하기 위해서 사용했다.
    • 이는 planner가 목표 허용 오차내의 solution cost를 찾는 순간 종료되었다.
  • 1,2는 무작위환경이며, 다양한 상태 차원에서의 문제를 테스트하기 위해서 사용했다.
    
    • 이는 복잡하고 높은 상태 차원에서 테스트하기 위해 사용했다. 알고리즘들에게 초기 솔루션을 개선할 수 있게 60초를 주었으며, 모든 실험의 각 변형마다 RRT*와 informed RRT*에게 동일한 의사 난수 시드와 지도를 사용하여 100회 실행했다.
    • 두 알고리즘은 최적화되지 않은 동일한 코드 기반을 공유하니 계산 시간의 상대적 비교가 가능하다.