Path Planning, Path Following, GraphSLAM

[신민서]

▶ Path Planning

Path Planning

: 로봇이 현재 위치에서 목표 지점까지 충돌 없이 이동할 수 있는 경로를 생성하는 문제

Global vs Local Path Planning 비교

	Global Planning	Local Planning
목표	지도 전체를 보고 출발지부터 목적지까지의 최적 경로 탐색	주행 중 센서로 감지된 동적 장애물 회피 및 경로 수정
기반 지도	전체 지도 (static map, 사전에 작성된 지도)	로봇 주변의 지도( local costmap, 실시간 생성)
장애물	벽, 기둥 등 고정된(static) 장애물 고려	보행자, 차량 등 움직이는(dynamic) 장애물 고려
갱신 주기	낮음 (경로를 크게 이탈하거나 최초 1회 갱신)	높음 (실시간, 보통 10-20Hz)
주요 알고리즘	Dijkstra, A*, RRT/RTT*	DWA, TEB, MPC

---

● Dijkstra (다익스트라) 알고리즘

: 그래프 이론에서 노드 간의 최단 경로를 찾는 가장 기초적인 알고리즘

작동 원리

- 시작점에서 연결된 모든 노드를 방문하며 비용(Cost)을 계산함

- 비용 함수: g(n) =cost(start -> n) // 시작점에서 현재 노드까지의 누적 비용만 고려

- 방문하지 않은 노드 중 비용이 가장 적은 노드를 선택해 반복 확장함

장점: 모든 가중치가 양수일 때 무조건 최적 경로를 찾아냄

단점: 목적지의 방향을 모르기 때문에 모든 방향으로 탐색 -> 연산량이 높고 속도 느림

---

●  A* (에이스타) 알고리즘

: Dijkstra에 휴리스틱(Heuristic)을 도입하여 탐색 효율을 높인 알고리즘

f(n) = g(n) + h(n)

- g(n): 시작점부터 현재 노드까지의 실제 이동 비율

- h(n): 현재 노드에서 목표점까지의 추정 잔여 거리

휴리스틱(h)의 종류

- Manhattan Distance: 격자 지도에서 상하좌우로만 이동할 때 사용

- Euclidean Distance: 대각선 이동이 가능할 때 직선거리 사용

목표 지점 방향으로 가중치를 주어 탐색하므로 속도가 빠름

---

●  RRT / RRT* (Rapidly-exploring Random Tree)

: 공간에 랜덤하게 점을 뿌려(Sampling) 트리를 확장해 나가는 방식

작동 원리

- 지도 상에 랜덤한 점(Random Sample)을 찍음

- 기존 트리(시작점에서 뻗어나온 선들)에서 샘플과 가장 가까운 노드를 찾음

- 그 방향으로 일정 거리만큼 가지를 뻗어 새로운 노드를 생성함

- 목표 지점에 도달할 때까지 반복

RRT vs RRT*

RRT	RRT*
경로가 지그재그로 거칠고 최적 경로가 아님 로봇 팔 같은 곳에서 매우 빠름	기존 RRT에 재배선(Rewiring) 과정을 추가함 주변 노드들을 검사해 더 짧은 경로가 있으면 연결을 바꿈 시간이 지날수록 직선에 가깝게 최적화 됨

복잡한 미로 같은 환경 탈출에 유리함

	Dihkstra	A*	RRT /RRT*
탐색 방식	완전 탐색(모든 방향)	휴리스틱 탐색(목표 지향)	랜덤 샘플링(무작위 확장)
속도	느림	빠름	빠름
최적성	보장	보장	RRT(x) RRT*(o)
지도 표현	Grid/Graph	Grid/Graph	연속 공간

---

▶ Path Following

Path Following

: Planner가 만든 경로(Path)를 로봇이 실제로 이탈하지 않고 따라가도록 선속도와 각속도를 제어하는 기술

input	output
로봇의 현재 위치와 자세 (Pose) 따라가야할 경로 (Waypoint)	속도 명령(cmd_vel): 선속도, 각속도

: 오차 최소화가 핵심

Path Following 오차 최소화 목표

- Cross-track Error (횡방향 오차): 경로의 중심선에서 로봇이 좌우로 얼마나 벗어나 있는가? (도로 중앙 잘 지키나)

- Heading Error (방향 오차): 로봇의 머리(Heading)가 경로의 진행 방향와 얼마나 틀어져 있는가?

---

● Pure Pursuit Controller

: 인간이 운전할 때 멀리 있는 한 점을 보고 핸들을 꺾는 것과 유사한 방식

작동 원리

- 로봇의 현재 위치에서 경로상 일정 거리 앞에 있는 점을 찾음

- 현재 위치에서 그 점을 원을 그리며 이동한다고 가정하고 조향각을 계산

---

● Stanley Controller

: 차량의 전륜(앞바퀴) 축을 기준으로 제어함

작동 원리

- 횡방향 오차와 헤딩 오차를 비선형적으로 결합하여 조향각을 산출함

특징

- Pure Pursuit보다 고속 주행 시 경로 추종 성능이 우수하고 오차가 적음

- 수식의 분모에 속도가 있어서 속도가 0에 가까우면 제어값이 무한대로 발산하여 급격히 튈 수 있음

---

● MPC (Model Predictive Control)

: 로봇의 물리 모델(Kinematics/Dynamics)을 이용해 미래를 예측하고 제어를 하는 기법

작동 원리

- 예측 (Prediction): 현재 상태에서 특정 제어 입력을 줬을 때, 미래 N초 동안 로봇이 어떻게 움직일지 시뮬레이션함

- 최적화 (Optimization): 경로와의 오차를 찾으면서도, 로봇의 물리적 한계를 고려해서 입력값을 찾음

- 실행: 계산된 미래의 입력값들 중 맨 첫 번째 값만 실행하고, 다음부터 다시 처음부터 계산

장점: 안정성이 높고 장애물 회피까지 통합 가능함

단점: 매 순간 복잡한 최적화 문제 연산 때문에 연산량이 높음

---

▶ GraphSLAM

GraphSLAM

: SLAM 문제를 그래프 형태의 최적화 문제로 모델링하여 푸는 방식

로봇이 SLAM을 수행하는 동안 센서 데이터가 입력으로 들어옴

순차적으로 들어오는 센서 데이터들의 차이를 통해 로봇의 포즈를 계산하는 알고리즘을 Odometry 또는 Front-end라고 함

이 때, 센서 데이터의 노이즈로 인해 Odometry는 에러가 누적됨 == 이러한 문제를 해결하기 위해 GraphSLAM이 나옴

GraphSLAM Pipeline

Front-End

: 로봇의 센서를 입력으로 받아 Pose Graph를 생성하는 부분, 시간이 지날수록 노이즈로 인한 에러가 누적됨

Back-End

: 누적된 에러를 최적화하는 부분, 사용 알고리즘 (PGO, Pose Graph Optimzation)

Pose Graph

: 로봇의 포즈를 자료구조 중 하나인 그래프로 표현한 SLAM에 특화된 자료구조

노드는 로봇의 Pose로 나타내고 엣지는 노드 사이의 상대 포즈로 나타냄

PGO가 수행되는 과정

1. Front-end에 의해 포즈를 계산한 후 pose graph를 순차적으로 생성

2. 로봇이 같은 장소를 재방문하면 loop detection 알고리즘이 작동

3. 루프가 탐지되면 기존 노드와 새로운 노드 사이에 loop edge가 생성됨

4. loop edge로 연결된 두 노드의 상대 포즈를 예측값으로 설정하고 두 노드의 센서 데이터를 매칭하여 관측값 생성

5. back-end에서 관측값과 예측값의 차이, relative pose 에러가 최소가 되는 방향으로 에러함수를 최적화함

이전 pose graph 노드들과 근접한 장소에 위치하면 loop detection 알고리즘으로 루프 탐지함

순차적으로 센서 데이터를 매칭하여 노드의 매칭 점수를 계산한다고 함

이후 매칭 점수가 가장 높은 노드와 loop closure edge를 생성함

이후 relative pose 에러가 최소가 되는 차량의 포즈를 업데이트 함...

https://alida.tistory.com/16

[SLAM] Pose Graph Optimization 개념 설명 및 예제 코드 분석