2022.10.6.(목) 공부 내용 - A*(D*), RRT, PRM 알고리즘

# 정리해야하는 알고리즘

- A*
- Pure Pursuit
- VFH(Vector Field Histogram)
- Potential Field
- Visibility Graph
- Voronoi Diagram
- PRM(Probabilistic Roadmap Method)
- RRT(Rapidly-exploring Random Tree)

 

🤖 [Book] Introduction to Autonomous Robots 내용 정리

4.2.3. A* (D*)

using Manhattan distance metric, which does not allow for diagonal movements.

A* and D*(an extension of A*, Dynamic A*) become computationally expensive when either the search space is large, e.g., due to a fine-grain resolution required for the task, or the dimensions of the search problem are high, e.g. when planning for an arm with multiple degrees of freedom. Solutions to these problems are provided by sampling-based path planning algorithms that are described further below.

 

A* 및 D*(A*의 확장, 동적 A*)는 작업에 필요한 세밀한 해상도로 인해 검색 공간이 크거나 여러 자유도를 가진 팔을 계획할 때처럼 검색 문제의 차원이 높을 때 계산 비용이 많이 든다. 이러한 문제에 대한 해결책은 아래에서 자세히 설명하는 샘플링 기반 경로 계획 알고리즘을 통해 제공된다.

 

4.3. Sampling-based Path Planning

In practice, most algorithms are only resolution complete, i.e., only complete if the resolution is fine-grained enough, as the state-space needs to be somewhat discretized for them to operate (e.g., into a grid) and some solutions might be missed as a function of the resolution of the discretization.

 

실제로는 대부분의 알고리즘이 작동하려면 상태 공간을 어느 정도 이산화(예: 그리드)해야 하고 이산화의 해상도에 따라 일부 솔루션이 누락될 수 있으므로 해상도가 충분히 세분화된 경우에만 해상도가 완전하다.

 

샘플링 기반의 알고리즘은 주어진 기하 공간 상에서 장애물들과 충돌하지 않는 로봇의 구성 상태 (Configuration state) 집합 V와, 이들을 잇는 간선의 집합 E를 컴포넌트로 갖는 그래프 G = (V, E)를 생성하는 것이 주요 목적이다.

 

실제로 대부분의 알고리즘은 분해능(resolution)이 완전하며, 즉 상태 공간을 어느 정도 이산화할 필요가 있으며 (예를 들어, 그리드 형태로) 이산화 분해능으로 일부 솔루션을 놓칠 수 있기 때문에 분해능이 충분히 세분화된 경우에만 완전하다.

 

샘플링 기반 플래너는 어떤 해를 찾거나 시간이 만료될 때까지 트리에 무작위로 점을 추가하여 가능한 경로를 생성합니다. 또한 샘플링 기반 경로 플래너는 확률적으로 완전합니다.

 

 Sampling-based Path Planning (샘플링 기반 경로 계획법) 은 형상 공간을 격자(grid)로 분할하지 않고, 랜덤(random)하게 샘플점을 여러 개 생성하여 점점이(point-wise) 공간을 탐색하여 경로를 찾아내는 방법이다. 

 

 즉, 형상 공간(configuration space) 내에서 샘플점을 무작위로 충분한 수만큼 발생시키고 그 샘플점이 혹은 두 개의 샘플점을 잇는 선이 장애물과 충돌하는지의 여부를 확인하여 자유 공간(free space)의 구조를 효율적으로 파악한다.

 

 Sampling-based algorithm의 랜덤 탐색은 특히 격자(grid) 생성 방법으로는 탐색이 거의 불가능한 고차원(high dimension) 공간에서 강력한 힘을 발휘한다. (저차원 공간에서의 문제들은 configuration space에 grid를 그려 계산하는 grid-based 알고리즘과 geometric 알고리즘 방식으로 해결 가능하다.)

 

- RRT (Rapidly-exploring Random Trees)

• Sampling-based Path Planning(샘플링 기반 경로 계획법) 중 하나이다.
•  RRT 알고리즘의 기본 아이디어는 트리(tree)라고 불리는 특별한 그래프를 구축하는 것으로, 형상 공간(configuration space)에서 시작점(initial point)과 목표점(goal point)을 연결한 트리를 산출한다.
• RRT는 탐색 영역 전체에 대해 랜덤으로 샘플점을 생성하고 시작점부터 트리를 서서히 성장시켜 가면서 도착점까지의  경로를 생성하는 알고리즘이다.
• RRT는 여러 가지 모양의 장애물이 있는 환경에서도 효율적으로 경로를 생성시켜주며, 비선형 동적 프로그래밍이나 혼합-정수 선형 프로그래밍 또는 확률 로드맵보다도 계산량이 훨씬 적다는 장점이 있다.
• 고차원 환경에서도 사용 가능하므로 활용성이 높다는 장점이 있다.

 

트리는 node(노드, vertex)와 edge(연결선)으로 구성된다.

트리 구조에서는 모든 노드는 부모(parent)라 불리는 1개의 다른 노드에 연결되며 시작점은 트리의 맨 상위 부모가 되는 root(루트)가 된다.

 

 그러나 여러 단점도 있다.

• 샘플링 개수가 충분하지 않다면 존재하는 경로를 찾지 못할 수도 있다.
  이것은 모든 샘플링 기반 경로계획법(sampling-based Path Planning)의 단점이라고 볼 수 있다.
• RRT 알고리즘으로 발생시킨 경로는 연결 가능(feasible)하지만, 최적(optimal)이지는 않다.
  (실제로 RRT 알고리즘에서 경로를 생성할 때, 어떠한 비용함수도 고려하지 않는다.)
• RRT 알고리즘은 도착점까지 수렴하는 데 많은 시간이 소요되며, 예측 가능하지 않다.
• 산출된 경로가 랜덤 샘플링의 성격상, 매우 삐뚤빼뚤하여 그대로는 구현이 불가능하다는 문제점이 있다.
• 로봇 또는 비행체의 운동학적 특성을 전혀 고려하지 않고 오로지 기하학적인 관계만을 이용했다.

 

RRT 알고리즘은 아래의 과정을 반복하여 실행한다.

 

ⅰ. 트리를 초기화하고 (트리를 비운다는 의미), 시작점(initial point)를 트리(tree)에 편입시킨다.

      따라서 시작점(q_ini)이 트리의 root(루트)가 된다. 이때 시작점은 루트로서 부모(parent) 노드가 없기 때문에 공집합이다.

ⅱ. 트리를 목표점(q_goal)에 연결될 때까지 확장시킨다. 우선 형상 공간(configuration space)에서 랜덤(random)하게 샘플점(q_rand)을 발생시킨 후, 트리(tree)에서 가장 가까운 노드(q_near)를 찾는다.

ⅲ. 샘플점과 가장 가까운 노드를 직선으로 연결한 선상에 트리(tree)로부터 미리 설정한 거리( γ)만큼 떨어진 새로운 점(q_new)을 트리로 편입시킨다. (만약 새로운 노드와 기존 노드를 연결한 직선, 즉 트리의 연결선(branch)이 장애물과 충돌한다면 새로운 샘플점을 랜덤하게 다시 발생시킨 후 같은 방법을 다시 시도한다.)

 

위 방법을 시작점(initial point)에서 시작하여 목표점(goal point)이 트리(tree)에 연결될 때까지 반복 시행하며 트리를 확장시켜 나간다. 이때, 트리(tree)가 고차원의 형상 공간(configurtion space) 속으로 신속하고 균일하게 확장되면서 자유 공간(free space)을 효과적으로 탐색하게 된다.

 

 

참고 자료를 바탕으로 직접 ppt를 만들어보았다.

시계 반대 방향 진행

위 그림은 시작점과 목표점 사이의 실현 가능한 경로가 발견될 때까지 무작위로 점을 샘플링하여 그래프에 연결하는 2D 검색 공간의 무작위적 탐색이다. 이것은 sampling-based planner가 어떻게 빠르게 넓은 영역을 탐색하고 시간의 흐름에 따라 solution을 정제해가는지를 잘 보여주는 예시이다. 또한 로봇의 시작점에서부터 그것의 가지가 목표점에 도달할 때까지 하나의 트리를 그려나가는 것으로 이해될 수 있다.  single-query path-planning algorithm이다.

 

RRT는 간단한 알고리즘이지만, 여러 모양의 장애물이 있는 복잡한 환경 속에서도 효율적으로 경로를 생성시켜주므로 로봇의 경로 계획에 많이 사용되고 있다. 최근에는 RRT의 여러 가지 단점을 극복한 확장판 알고리즘이 많이 개발되면서 무인기의 경로 계획에도 적용되기 시작했다.

 

- PRM (Probabilistic RoadMaps)

상태 공간에서 무작위로 점을 샘플링하고 충돌이 없는지 테스트한 다음 로봇의 운동학을 반영하는 경로를 사용하여 인접 지점과 연결한 다음 고전적인 그래프 최단 경로 알고리즘을 사용하여 결과 구조에서 최단 경로를 찾아 트리를 만든다.

이것의 장점은 환경이 변하지 않는다는 가정 하에, 오직 한번 생성되고 나면 여러 쿼리에 사용될 수 있다는 것이다.

따라서 multi-query path-planning algorithm이다.

 

 

A probabilistic roadmap (PRM) is a network graph of possible paths in a given map based on free and occupied spaces.

“확률적 로드맵(PRM)은 빈 공간과 점유 공간을 기반으로 주어진 지도에서 가능한 경로를 나타내는 네트워크 그래프이다.”

 

4.3.1. Basic Algorithm

 

- X : d-dimensional state-space, 이것은 변환 및 회전(6차원) 측면에서 주어진 로봇의 상태가 될 수 있으며, 가능한 관절 각도당 1차원의 관절 공간이 될 수도 있다.

- G : G ⊂ X, 목표로 여겨지는 상태 공간에서의 d차원의 구체로 둔다.

- t : the allowed time.

 

위 과정은 결과적 트리에서 시간이 허락되는 대로 반복될 수 있다. 이것은 Anytime algorithm으로도 알려진다.

적절한 거리 계량이 주어지면 목표까지의 비용 값을 트리의 각 노드에 저장할 수 있다. (목표지점에서 시작지점까지 트리를 그려나가면 훨씬 쉽다.) 또한 그것은 가장 짧은 경로를 쉽게 검색할 수 있도록 한다.

 

4 Keypoints in this algorithm..

1. 트리에 추가하거나 혹은 제거하기 위해 다음 포인트를 찾는다. (StateToExpandFrom)

2. 찾은 포인트를  로봇의 운동학적인 부분을 고려하여 트리에 어떻게 연결할지 찾는다. (CreatePathToTree)

3. 찾은 이 경로가 적절한지 테스트한다. 즉, 충돌이 없는지 확인한다. (collision-free)

4. 추가하기 위한 다음 포인트를 찾는다.

 

prominent method는 상태공간에서의 임의의 포인트를 선정하여 트리에서 가까이 존재하는 점에 연결하거나 목표점에 연결하는 것이다. 이것은 트리에 있는 모든 노드에 대해 검색하고, 유력한 점까지의 거리를 계산할 것을 필요로 한다.

다른 접근 방식들은 더 적은 out-degrees(out-sourcing)을 가진 (아직 많은 연결을 가지지 않은) 노드를 선호하고 주변에서의 새로운 포인트를 선택했다. 두 접근법은 전체 상태 공간을 빠르게 탐색할 수 있도록 한다.

예를 들어 로봇이 컵을 들어야 하므로 손목을 회전하지 않아야 하는 등 로봇의 경로에 제약이 있는 경우 이 치수는 상태 공간에서 간단히 제거할 수 있다.

 

가능한 경로를 찾았을 때, 이 공간은 최대 경로 길이로 제한되는 타원체로 줄어든다.

이 타원체는 시작점과 목표점의 최대 경로 길이의 와이어를 장착하고 펜으로 바깥쪽으로 밀어낼 수 있다.

(와이어로 제한된) 펜으로 도달할 수 있는 모든 공간은 더 짧은 경로로 이어질 수 있는 점을 포함한다.

이 접근법은 같은 planner의 여러 복사본을 병렬로 동작시키고 발견된 최단 경로를 교환할 때 특히 효과적이다.

 

 

- translation and rotation of coordinates

 

- Anytime algorithm

: 문제가 끝나기 전에 중단되더라도 문제에 대한 유효한 솔루션을 반환할 수 있는 알고리즘.

대부분의 알고리즘은 완료될 때까지 실행되지 않으면 유용한 솔루션 정보를 제공할 수 없으나 Anytime algorithm은 사용자가 완료 전에 알고리즘을 종료하길 원한다면 (수행한 계산의 양에 따라 품질은 달라지지만) 부분적 답변을 반환해준다. 이때 알고리즘에 의해 생성된 솔루션은 정답의 근사치이다.

참고: https://en.wikipedia.org/wiki/Anytime_algorithm

 

# 참고 자료

[Introduction to autonomous robots - Nikolaus Correll]

https://stackoverflow.com/questions/49967249/how-to-translate-and-rotate-coordinates

How to translate and rotate coordinates?

 

https://velog.io/@al_potato/D-%EC%95%8C%EA%B3%A0%EB%A6%AC%EC%A6%98

D* 알고리즘

https://pasus.tistory.com/74

급속탐색 랜덤트리 (RRT, rapidly-exploring random tree)

https://velog.io/@delicate1290/ROS2-RRT-%EC%95%8C%EA%B3%A0%EB%A6%AC%EC%A6%98

ROS2 - RRT 알고리즘

(아래 페이지에 RRT 실행 동영상이 있다.)

https://dowtech.tistory.com/24

RRT 알고리즘 (Rapidly exploring Random Tree, 예전자료)

https://ropiens.tistory.com/9

Motion planning - 3. 알고리즘들

https://hashdork.com/ko/algorithms-used-in-robotics-explained/

로봇 공학에 사용되는 알고리즘 - 설명 - HashDork

https://robodk.com/blog/prm-motion-planner/

How it Works: RoboDK's New PRM Motion Planner - RoboDK blog

https://theclassytim.medium.com/robotic-path-planning-prm-prm-b4c64b1f5acb

Robotic Path Planning: PRM & PRM*

 

https://kr.mathworks.com/help/nav/ug/plan-mobile-robot-paths-using-rrt_ko_KR.html

RRT를 사용한 이동 로봇 경로 계획 - MATLAB & Simulink - MathWorks 한국

https://kr.mathworks.com/help/nav/ug/choose-path-planning-algorithms-for-navigation.html

내비게이션을 위한 경로 계획 알고리즘 선택하기 - MATLAB & Simulink - MathWorks 한국

 

*아래 페이지에 PRM 알고리즘이 아주 잘 설명되어있다.

https://medium.com/acm-juit/probabilistic-roadmap-prm-for-path-planning-in-robotics-d4f4b69475ea

Probabilistic Roadmap (PRM) for Path Planning in Robotics

 

#Sampling-based Path-Planning 관련 참고 공부 자료

[Sampling-based Algorithms for Optimal Motion Planning] 정리 자료

https://jdj2261.github.io/review/2021/10/07/rrt-star-review.html

djjin