RRT, RRT* 구현 및 결과 확인

0. 개요

- 참고 논문

위 논문의 sudo코드와 '프로그래머스' 교육 자료를 기반으로 RRT와 RRT*에 대해 공부한 것을 기록.

*RRT : Rapidly exploring Random Tree

 

- 사용한 파이썬 패키지

import numpy as np
import matplotlib.pyplot as plt
import networkx as nx

numpy - 숫자 데이터를 다루는데 유용한 패키지

matplotlib - 시각화 tool

networkx - 그래프를 다루는 유용한 패키지

 

1. RRT 구현

1) sudo 코드

2) python 구현

G = nx.DiGraph()
G.add_nodes_from([
    (-1,{"x": start[0], "y": start[1]})
])
max_iterations = 100
goal_node = None
for i in range(max_iterations):
  node_rand = sampleFree(space)
  node_nearest, node_nearest_id = nearest(G, node_rand)
  node_new = steer(node_nearest, node_rand)
  
  if obstacleFree(node_nearest, node_new, obstacles):
    G.add_nodes_from([
        (i, node_new)
    ])
    G.add_edge(node_nearest_id, i)
  
    if is_goal(node_new, goal):
      goal_node_id = i
      break

sampleFree 함수 : Free space에서 샘플링하여 랜덤 노드를 찍어주는 함수

nearest 함수 : 랜덤 노드와 현재 노드의 거리 중 가장 가까운 노드를 찾는 함수

steer 함수 : nearest -> rand 방향으로 적정 step 만큼 새로운 노드를 정의하는 함수

obstacleFree 함수 : 새로운 노드가 장애물에 부딪히는지 확인하는 함수

is_goal 함수 : 새로운 노드가 목표 지점에 도착했는지 확인하는 함수

 

3) 결과

디버깅의 편의를 위해 랜덤 seed를 특정 값으로 고정.

보다 쉽게 결과를 확인하기 위해 matplotlib 라이브러리로 시각화.

parameter들을 바꿔가면서 확인.

 

- max_iterations(반복 수)

100번의 반복으로는 goal 지점을 찾기에 부족했다. 그래서 max_iterations 파라미터를 200으로 설정하니 177회에 goal 지점에 도달할 수 있었고 붉은 색의 optimal path를 return 받을 수 있었다.

 

 

- step size

 goal 지점에 도달하기 위해서 불필요하게 많은 step이 소요되었다. 그래서 Steer() 함수의 고정 step size를 변경하였다. numpy의 random.uniform() 메서드를 사용하여 step size를 가변하게 설정하였다. 설정 값은 figure 상단에 표기된 것처럼 1.0~5.0의 값이 랜덤으로 설정되게 하였다. 결과는 177번만에 도달하는 결과가 90번만으로 도달한 결과를 보였다.

 

 

- sampling style

이번에는 sampling의 방식을 바꾸어 결과를 확인하였다. 수정은 샘플링을 담당하는 sampleFree() 함수에서 변경이 있었고 기존 sampling은 그저 랜덤한 x, y 좌표를 샘플링했다면 변경된 'goal sampling'은 70%는 랜덤으로 x,y 나머지 30%는 goal 좌표를 샘플링하여 기존보다 goal 지점으로 가려는 성향을 크게 하였다. 결과를 보면 90번의 반복이 73번만에 도착하는 결과를 보인다.

 

2. RRT*

1) sudo 코드

naive RRT에서 cost min select 파트와 Rewiring 부분이 추가되었다.

  - cost min select 파트 : new 노드와 near 노드들을 비교해 가장 낮은 cost를 가지도록 연결(edge)

  - rewiring 파트 : 기존 near 노드의 cost가 최소가 되도록 재연결

 

2) python 구현

node에 cost 정보도 주어야 했고 업데이트를 할 수 있어야 했다. RRT*에 대한 코드는 추가적으로 링크를 남기는 것으로.

 

3) 결과 비교

- naive RRT vs. not rewiring RRT*

기존 RRT와 cost min select 파트만을 구현한 RRT*를 비교한 결과이다. 둘다 390번의 반복으로 goal 지점에 도달하는 하였지만, RRT*의 optimal path(붉은 선)가 좀 더 유용한(?) 결과를 보인다. 가운데 상단 부근이 가장 두드러지는 결과를 보이는데, 'ㄷ'자로 꺾인 기존 RRT의 path에 반해 RRT*는 수직으로 곧게 뻗은 path를 갖는다.

 

 

- not rewiring RRT* vs. RRT*

near 노드들의 cost를 확인하여 cost가 작은 쪽으로 edge가 재연결되도록 함을 볼 수 있다. 하지만 optimal path의 변화가 눈에 두드러지게 나타나지는 않았다.

 

+ rewiring 확인

특정 부분을 확대하여 rewiring이 어떤 영향을 주는지 확인해 보았다. 해당 부분을 보면 비록 path에 영향을 주는 부분은 아니지만 291, 237번 노드가 rewiring 되어 보다 짧은 edge를 가지게 됨을 알 수 있었다.

 

 

- RRT*(break) vs. RRT*(no break)

이번에는 충분히 정해진 iter 만큼 반복할 수 있다는 가정하에 goal 지점에 도착해도 정해진 iter만큼 rewiring을 계속 수행하도록 구성하였고 그 결과를 나타낸다. 확실히 노드들이 많아졌지만 연산을 한 만큼 path도 더욱 부드러워진 결과를 보인다. => iter 파라미터를 적절히 튜닝이 필요함을 알 수 있었다.

 

 

- 정리

optimal path만을 나타낸 결과이다. 확실히 cost min select 파트와 rewired 파트를 진행하면서 부드러운 path를 보임을 한눈에 확인할 수 있었다.