모션 플래닝과 경로 계획

1모션 플래닝 개요

모션 플래닝(Motion Planning)은 로봇이 시작 위치에서 목표 위치까지 충돌 없이 이동하는 경로를 찾는 문제입니다. 6축 로봇의 경우 6차원 구성 공간(Configuration Space)에서 탐색이 이루어집니다. 전통적인 샘플링 기반 방법(RRT, PRM)부터 최신 딥러닝 기반 방법까지 다양한 접근법이 있습니다.

모션 플래닝 알고리즘 프레임워크

시작 위치

q_s

목표 위치

q_g

↓

Configuration Space (C-Space)

RRT*

샘플링

A*

그래프

MPC

최적화

RL

학습

↓

충돌 검사기

Collision Checker

↓

최적 경로

Path / Trajectory

Python - Motion Planning Framework
from dataclasses import dataclass
from typing import List, Tuple, Optional
import numpy as np

@dataclass
class RobotConfiguration:
    """로봇 관절 구성"""
    joint_angles: np.ndarray  # 6-DOF: [j1, j2, j3, j4, j5, j6]

    def distance_to(self, other: 'RobotConfiguration') -> float:
        """구성 공간에서의 거리"""
        return np.linalg.norm(self.joint_angles - other.joint_angles)

class CollisionChecker:
    """충돌 검사 모듈"""

    def __init__(self, robot_model, obstacles: List):
        self.robot = robot_model
        self.obstacles = obstacles

    def is_collision_free(self, config: RobotConfiguration) -> bool:
        """주어진 구성에서 충돌 여부 확인"""
        # Forward Kinematics로 각 링크 위치 계산
        link_positions = self.robot.forward_kinematics(config.joint_angles)

        # 각 링크와 장애물 간 충돌 검사
        for link in link_positions:
            for obstacle in self.obstacles:
                if self._check_collision(link, obstacle):
                    return False
        return True

2RRT (Rapidly-exploring Random Tree)

RRT는 고차원 공간에서 효율적으로 경로를 탐색하는 샘플링 기반 알고리즘입니다. 무작위 샘플을 생성하고 가장 가까운 노드에서 확장하여 트리를 성장시킵니다. RRT*는 최적성을 보장하는 개선 버전으로, 새 노드 추가 시 주변 노드들의 부모를 재연결(rewire)하여 비용을 줄입니다.

Python - RRT* Algorithm
import random
from typing import Optional

class RRTStar:
    """RRT* 경로 계획 알고리즘"""

    def __init__(self, collision_checker: CollisionChecker,
                 step_size: float = 0.1,
                 neighbor_radius: float = 0.5):
        self.collision_checker = collision_checker
        self.step_size = step_size
        self.neighbor_radius = neighbor_radius

    def plan(self, start: RobotConfiguration,
               goal: RobotConfiguration,
               max_iterations: int = 5000) -> Optional[List]:
        """RRT* 경로 계획"""
        # 트리 초기화
        self.tree = {0: {'config': start, 'parent': None, 'cost': 0.0}}

        for i in range(max_iterations):
            # 1. 무작위 샘플 (10% 목표 편향)
            q_rand = goal if random.random() < 0.1 else self._random_sample()

            # 2. 가장 가까운 노드 찾기
            nearest_id = self._nearest_neighbor(q_rand)

            # 3. 새 노드로 확장
            q_new = self._steer(self.tree[nearest_id]['config'], q_rand)

            # 4. 충돌 검사
            if self.collision_checker.is_collision_free(q_new):
                # 5. 최소 비용 부모 선택 (RRT* 핵심)
                neighbors = self._find_near_nodes(q_new)
                best_parent = self._choose_best_parent(q_new, neighbors)

                # 6. 노드 추가 및 재연결
                new_id = self._add_node(q_new, best_parent)
                self._rewire(new_id, neighbors)

                # 7. 목표 도달 확인
                if q_new.distance_to(goal) < self.step_size:
                    return self._extract_path(new_id)

        return None

3A* 기반 그래프 탐색

A* 알고리즘은 휴리스틱을 활용한 최적 경로 탐색 알고리즘입니다. 구성 공간을 격자로 이산화하거나, PRM(Probabilistic Roadmap)으로 로드맵을 구축한 후 A*로 탐색합니다. 로봇 경로 계획에서는 관절 공간 거리나 작업 공간 거리를 휴리스틱으로 사용합니다.

Python - A* for Robot Path Planning
import heapq

class AStarPlanner:
    """A* 기반 로봇 경로 계획"""

    def plan(self, start_id: int, goal_id: int) -> Optional[List]:
        """A* 경로 탐색"""
        open_set = [(self._heuristic(start_id, goal_id), start_id)]
        came_from = {}
        g_score = {start_id: 0}

        while open_set:
            _, current = heapq.heappop(open_set)

            if current == goal_id:
                return self._reconstruct_path(came_from, current)

            for neighbor, edge_cost in self.roadmap[current]['neighbors']:
                tentative_g = g_score[current] + edge_cost

                if tentative_g < g_score.get(neighbor, float('inf')):
                    came_from[neighbor] = current
                    g_score[neighbor] = tentative_g
                    f = tentative_g + self._heuristic(neighbor, goal_id)
                    heapq.heappush(open_set, (f, neighbor))

        return None

    def _heuristic(self, node_id: int, goal_id: int) -> float:
        """휴리스틱: 관절 공간 유클리드 거리"""
        return np.linalg.norm(
            self.roadmap[node_id]['config'] - self.roadmap[goal_id]['config']
        )

4강화학습 기반 경로 최적화

강화학습(RL)은 경험을 통해 최적 경로를 학습합니다. 기존 알고리즘이 단일 쿼리에 대한 해를 찾는다면, RL은 다양한 상황에서 일반화된 정책을 학습합니다. 특히 동적 환경이나 복잡한 제약 조건에서 유용합니다.

Python - RL-based Motion Planning
import torch
import torch.nn as nn

class MotionPlanningPolicy(nn.Module):
    """경로 계획을 위한 정책 네트워크"""

    def __init__(self, state_dim: int, action_dim: int):
        super().__init__()
        self.encoder = nn.Sequential(
            nn.Linear(state_dim, 256),
            nn.ReLU(),
            nn.Linear(256, 256),
            nn.ReLU()
        )
        self.actor = nn.Sequential(
            nn.Linear(256, 128),
            nn.ReLU(),
            nn.Linear(128, action_dim),
            nn.Tanh()
        )

    def forward(self, state: torch.Tensor) -> torch.Tensor:
        features = self.encoder(state)
        return self.actor(features)

class RLMotionPlanner:
    """강화학습 기반 모션 플래너"""

    def compute_reward(self, state, action, next_state, goal) -> float:
        """보상 함수 설계"""
        # 목표 접근 보상
        dist_to_goal = np.linalg.norm(next_state[:6] - goal)
        prev_dist = np.linalg.norm(state[:6] - goal)
        progress_reward = prev_dist - dist_to_goal

        # 충돌 페널티
        collision_penalty = -10.0 if self._check_collision(next_state) else 0.0

        # 목표 도달 보너스
        goal_bonus = 100.0 if dist_to_goal < 0.05 else 0.0

        return progress_reward + collision_penalty + goal_bonus

5알고리즘 비교

각 알고리즘은 장단점이 있어 상황에 맞는 선택이 필요합니다. 실무에서는 RRT*로 초기 경로를 찾고, 궤적 최적화로 시간/에너지를 최적화하는 파이프라인이 일반적입니다.

알고리즘	특징	장점	단점
RRT*	샘플링 기반	고차원 효율적	최적성 수렴 느림
A*	그래프 탐색	최적 경로 보장	이산화 필요
RL	학습 기반	일반화 능력	학습 시간 필요

실무 적용: 실제 시스템에서는 RRT*로 초기 경로를 찾고, TOPP(Time-Optimal Path Parameterization)로 궤적을 최적화합니다.