# 경로 계획 및 네비게이션 시뮬레이션

경로 계획과 네비게이션은 로봇이 주어진 환경에서 목표 위치로 이동하기 위해 필수적인 요소이다. 이 과정은 로봇의 현재 상태와 환경 정보를 바탕으로 최적의 경로를 생성하고, 해당 경로를 따라 자율적으로 이동하도록 제어하는 일련의 과정을 포함한다.

#### 1. 경로 계획의 개념

경로 계획은 로봇이 장애물을 피해 목표 지점까지 안전하고 효율적으로 이동할 수 있는 경로를 결정하는 과정이다. 이 과정에서 주요하게 다루는 개념은 다음과 같다:

* **작업 공간**: 로봇이 이동할 수 있는 실제 환경으로, 장애물과 자유 공간을 포함한다.
* **구성 공간 (C-space)**: 로봇의 모든 가능한 위치와 자세를 정의하는 공간으로, 작업 공간 내의 장애물과 로봇의 충돌을 피하는 위치들을 정의한다.

경로 계획 문제는 보통 로봇의 이동 경로를 수학적으로 모델링하고 이를 최적화하는 문제로 변환된다.

#### 2. 경로 계획 문제 정의

경로 계획 문제는 시작 위치 $\mathbf{x}*\text{start}$에서 목표 위치 $\mathbf{x}*\text{goal}$까지의 경로 $\mathbf{P}(t)$를 찾는 문제로 정의된다. 여기서, $\mathbf{P}(t)$는 시간 $t$에 따라 로봇의 상태를 나타내는 함수이다. 경로는 다음 조건들을 만족해야 한다:

1. **안전성**: 경로는 작업 공간 내의 장애물들과 충돌하지 않아야 한다.
2. **연속성**: 경로는 연속적이어야 하며, 로봇의 동역학 제약을 충족해야 한다.
3. **최적성**: 주어진 비용 함수 $J(\mathbf{P})$에 대해 최적의 경로를 찾아야 한다.

#### 3. 경로 계획 알고리즘

경로 계획 알고리즘은 크게 **전역 경로 계획**과 **지역 경로 계획**으로 나뉜다. 전역 경로 계획은 로봇이 전체 작업 공간에서 목표로 가는 경로를 찾는 것을 의미하며, 지역 경로 계획은 주어진 시간에서 로봇의 주변 환경만을 고려하여 즉각적인 경로를 찾는 것을 의미한다.

**전역 경로 계획**

전역 경로 계획은 작업 공간의 전체 정보를 기반으로 최적 경로를 찾는 방법이다. 대표적인 알고리즘으로는 **A\*** 알고리즘과 **Dijkstra** 알고리즘이 있다.

* **A\*** 알고리즘: A\*는 휴리스틱 함수 $h(\mathbf{x})$를 사용하여 경로의 비용을 추정하고, 목표에 더 가까운 경로를 우선 탐색하는 방식이다. 탐색 비용은 다음과 같이 정의된다:

$$
f(\mathbf{x}) = g(\mathbf{x}) + h(\mathbf{x})
$$

여기서, $g(\mathbf{x})$는 현재까지의 경로 비용, $h(\mathbf{x})$는 목표까지의 예상 비용이다.

* **Dijkstra 알고리즘**: Dijkstra 알고리즘은 휴리스틱을 사용하지 않고, 단순히 모든 노드에 대한 경로 비용을 최소화하는 방식이다. 따라서 A\*보다 느리지만 항상 최적 경로를 보장한다.

**지역 경로 계획**

지역 경로 계획은 로봇의 현재 상태와 가까운 주변 정보만을 기반으로 즉각적인 경로를 찾는 방법이다. 대표적인 알고리즘으로는 \*\*Dynamic Window Approach (DWA)\*\*와 **Potential Field**가 있다.

* **DWA**: DWA는 로봇의 동역학 모델을 기반으로 주어진 시간 창 내에서 로봇이 이동할 수 있는 가능한 속도를 고려하여 최적 경로를 계산한다. DWA의 비용 함수는 다음과 같이 정의된다:

$$
J(\mathbf{v}, \mathbf{\omega}) = \alpha \cdot J\_\text{heading} + \beta \cdot J\_\text{velocity} + \gamma \cdot J\_\text{obstacle}
$$

여기서, $\mathbf{v}$와 $\mathbf{\omega}$는 각각 로봇의 선속도와 각속도를 나타내며, 각 비용 항목은 경로의 목표 방향, 속도, 장애물과의 거리 등을 고려한다.

* **Potential Field**: Potential Field 방법은 목표 지점이 로봇을 끌어당기는 **인공 포텐셜**을 만들고, 장애물이 로봇을 밀어내는 **반발 포텐셜**을 생성하는 방식으로 경로를 계획한다. 로봇이 따라가야 할 경로는 다음과 같이 정의된다:

$$
\mathbf{F} = \mathbf{F}*\text{attract} + \mathbf{F}*\text{repulse}
$$

여기서, $\mathbf{F}*\text{attract}$는 목표 지점으로부터의 인력, $\mathbf{F}*\text{repulse}$는 장애물로부터의 반발력을 나타낸다.

#### 4. 로봇의 상태와 경로 추종 제어

경로를 계획한 후, 로봇이 해당 경로를 정확히 따르기 위해서는 **경로 추종 제어**가 필요하다. 경로 추종 제어는 로봇의 현재 위치 $\mathbf{x}(t)$와 목표 위치 $\mathbf{x}\_\text{desired}(t)$ 사이의 오차를 줄이는 제어 방법을 말한다.

경로 추종 제어의 기본 원리는 **오차 역학**을 정의하고, 이 오차를 최소화하는 제어 입력을 계산하는 것이다. 오차는 다음과 같이 정의된다:

$$
\mathbf{e}(t) = \mathbf{x}\_\text{desired}(t) - \mathbf{x}(t)
$$

주어진 오차를 기반으로 제어 입력 $\mathbf{u}(t)$를 계산하는 방법에는 여러 가지가 있다. 대표적인 방법으로는 \*\*비례-적분-미분 제어 (PID)\*\*와 \*\*모델 예측 제어 (MPC)\*\*가 있다.

**PID 제어**

PID 제어는 오차의 비례항, 적분항, 미분항을 사용하여 제어 입력을 생성하는 방식이다. PID 제어의 제어 입력은 다음과 같이 정의된다:

$$
\mathbf{u}(t) = K\_P \mathbf{e}(t) + K\_I \int \mathbf{e}(t) dt + K\_D \frac{d\mathbf{e}(t)}{dt}
$$

여기서, $K\_P$, $K\_I$, $K\_D$는 각각 비례, 적분, 미분 이득이다.

**모델 예측 제어 (MPC)**

모델 예측 제어는 주어진 로봇의 동역학 모델을 기반으로 미래의 경로를 예측하고, 최적의 제어 입력을 계산하는 방법이다. MPC는 일정 시간 구간 $T$ 동안 미래의 상태를 예측하여 비용 함수 $J(\mathbf{u})$를 최소화하는 제어 입력 $\mathbf{u}(t)$를 구한다. MPC의 비용 함수는 다음과 같이 정의된다:

$$
J(\mathbf{u}) = \sum\_{t=0}^{T} \left( |\mathbf{x}\_\text{desired}(t) - \mathbf{x}(t)|^2 + \lambda |\mathbf{u}(t)|^2 \right)
$$

여기서, 첫 번째 항은 목표 경로와 현재 경로 사이의 오차를 최소화하며, 두 번째 항은 제어 입력의 크기를 최소화한다. $\lambda$는 제어 입력의 가중치를 결정하는 파라미터이다.

MPC는 로봇의 동역학 모델을 명시적으로 사용하여 미래의 경로를 예측하기 때문에 매우 정밀한 경로 추종 성능을 보장할 수 있다. 그러나 계산 복잡도가 높아 실시간 제어에서는 계산 자원을 많이 요구할 수 있다.

#### 5. 네비게이션 시뮬레이션

경로 계획이 완료되면, 로봇은 해당 경로를 따라 자율적으로 이동해야 한다. 이를 위해 **네비게이션** 시스템이 필요하며, 주로 **SLAM (Simultaneous Localization and Mapping)** 또는 \*\*VSLAM (Visual SLAM)\*\*과 같은 기술을 통해 로봇의 실시간 위치를 추정하고, 주변 환경을 맵핑한다.

**SLAM 개요**

SLAM은 로봇이 알 수 없는 환경에서 자신의 위치를 추정하면서 동시에 환경의 지도를 작성하는 문제이다. SLAM 문제는 크게 두 가지 하위 문제로 나눌 수 있다:

* **위치 추정**: 로봇의 현재 위치를 추정하는 문제로, 보통 확률적 필터 (예: **확장 칼만 필터 (EKF)**)를 사용하여 로봇의 위치를 추정한다.
* **지도 작성**: 로봇이 탐색한 공간의 지도를 생성하는 문제로, 보통 **Occupancy Grid Map** 또는 **Feature-based Map**을 사용하여 환경을 모델링한다.

SLAM에서 위치 추정은 센서 데이터를 기반으로 하며, 센서 데이터는 일반적으로 **LIDAR**, **카메라**, **IMU** 등의 센서로부터 수집된다.

**경로 계획과 SLAM의 결합**

SLAM과 경로 계획은 네비게이션 시스템의 핵심 구성 요소로, 로봇이 목표 지점까지의 경로를 계획하고, 동시에 실시간으로 자신의 위치를 추정하면서 이동한다. 이 과정에서 경로 계획 알고리즘과 SLAM 시스템이 서로 상호작용하여 경로를 수정하고, 장애물을 회피하는 능력을 갖출 수 있다.

SLAM을 통한 위치 추정은 보통 다음과 같은 수식으로 표현된다:

$$
\mathbf{x}*t = f(\mathbf{x}*{t-1}, \mathbf{u}\_t) + \mathbf{w}\_t
$$

$$
\mathbf{z}\_t = h(\mathbf{x}\_t) + \mathbf{v}\_t
$$

여기서, $\mathbf{x}\_t$는 로봇의 현재 상태 벡터, $\mathbf{u}\_t$는 제어 입력, $\mathbf{z}\_t$는 센서 측정값을 나타낸다. $\mathbf{w}\_t$와 $\mathbf{v}\_t$는 각각 시스템 노이즈와 센서 노이즈이다.

#### 6. 네비게이션 스택 구성

로봇 네비게이션을 위해 다양한 소프트웨어 스택을 구성할 수 있다. 대표적으로 \*\*ROS (Robot Operating System)\*\*에서 제공하는 **Navigation Stack**은 경로 계획, 장애물 회피, SLAM을 통합한 네비게이션 시스템을 구축할 수 있는 강력한 도구이다.

ROS 네비게이션 스택의 주요 구성 요소는 다음과 같다:

* **global planner**: 전역 경로 계획을 담당하며, 목표 위치까지의 최적 경로를 생성한다.
* **local planner**: 지역 경로 계획을 담당하며, 주어진 시간에서 로봇의 주변 장애물을 고려하여 경로를 수정한다.
* **costmap**: 작업 공간의 지도를 생성하고, 장애물의 위치를 관리하는 역할을 한다.

네비게이션 스택을 활용하면, 로봇이 SLAM을 통해 실시간으로 환경을 인식하고, 생성된 지도를 바탕으로 경로 계획을 수행하여 자율 주행을 할 수 있다.

#### 7. 경로 계획 및 네비게이션의 실시간 적용

로봇이 경로를 계획하고, 자율적으로 네비게이션을 하기 위해서는 실시간으로 주어진 환경을 파악하고 경로를 조정해야 한다. 이때 중요한 개념은 \*\*재계획(replanning)\*\*이다. 로봇이 예상치 못한 장애물을 만나거나, 환경이 변화할 경우 기존 경로를 실시간으로 수정하여 새로운 경로를 계획해야 한다.

재계획은 보통 다음과 같은 수식으로 표현된다:

$$
\mathbf{P}\_\text{new}(t) = \text{Replan}(\mathbf{P}(t), \mathbf{z}\_t)
$$

여기서, $\mathbf{P}\_\text{new}(t)$는 새로 계획된 경로를, $\mathbf{P}(t)$는 기존 경로를, $\mathbf{z}\_t$는 센서로부터 얻은 환경 정보를 나타낸다.

로봇이 재계획을 수행할 때, 전역 경로 계획과 지역 경로 계획이 상호작용하여 새로운 경로를 생성하게 된다. 이러한 과정을 통해 로봇은 장애물을 피하면서 목표 지점까지 안전하게 도착할 수 있다.