SLAM과 모션 플래닝으로 구성하는 자율 로봇의 위치 추정과 경로 제어

위치 추정과 경로 제어는 하나의 폐루프로 동작한다

미지의 공간을 이동하거나 물체를 조작하는 로봇은 현재 위치를 알아야 하고, 주변 환경을 지도화해야 하며, 그 결과를 바탕으로 안전한 움직임을 계속 갱신해야 한다. SLAM(Simultaneous Localization and Mapping)과 Motion Planning(경로·궤적 계획)은 이 폐루프를 구성하는 핵심 알고리즘이다.

SLAM은 카메라, LiDAR, IMU, 휠 오도메트리처럼 서로 다른 센서의 관측을 이용해 로봇 포즈와 환경 지도를 동시에 추정한다. Motion Planning은 시작 상태에서 목표 상태까지 동역학과 각종 제약을 만족하면서 충돌 없는 경로나 시간-최적 궤적을 산출한다. 전자는 지도와 위치의 신뢰도를, 후자는 실제 이동의 안전성과 실행 가능성을 담당한다.

센서 관측을 전역적으로 일관된 지도와 포즈로 연결하는 SLAM

SLAM은 프런트엔드와 백엔드로 역할을 나눠 구성한다. 프런트엔드에서는 특징점·라인·서페이스를 추출하거나 스캔 매칭을 수행한다. 시각 SLAM에서는 키프레임 관리가, LiDAR SLAM에서는 ICP/NDT가 활용된다.

백엔드는 Factor Graph 기반 비선형 최소화로 전역 일관성을 유지한다. 이 단계에서 g2o, Ceres, GTSAM 같은 도구를 사용할 수 있으며, 루프 클로저 감지와 Pose Graph 정합이 누적 드리프트를 줄이는 역할을 한다. 위치를 잃었을 때는 재지역화가 프런트엔드와 다시 연결된다.

지도 표현도 작업 목적에 맞춰 고른다. Occupancy Grid, Point Cloud, ESDF/TSDF는 각각 내비게이션과 조작에 필요한 정보를 다르게 제공한다. 해상도, 메모리, 정확도 사이의 트레이드오프를 고려해야 한다.

전역 탐색과 로컬 제어를 잇는 모션 플래닝

모션 플래닝은 전역 계획과 로컬 계획·추종으로 계층화한다. 전역 계획에서는 지식 기반 탐색이나 샘플링 기반 기법으로 연결 가능성을 확보하고, 준최적 경로를 만든다. PRM*/RRT*는 대표적인 샘플링 기반 기법이며, 비용 함수에는 길이·안전 여유·에너지 같은 요소를 함께 반영할 수 있다.

로컬 단계에서는 DWA/TEB, MPC가 동역학 제약을 반영하면서 실시간 회피와 추종을 수행한다. 모델 불일치와 센서 지연에는 예측 제어와 게인 스케줄링으로 대응한다. 충돌이 예측되거나 비용이 급등하고 지도가 바뀌면 트리밍·재샘플링을 수행하며, 안전 정지와 페일세이프 경로도 보장해야 한다.

지도 갱신부터 재계획까지의 연결

⤢✕

기법별 선택 기준

카테고리	대표 기법	성능(정확도/최적성)	확장성(대규모/실시간)	일관성(전역/루프)	안정성(잡음/동적물)	운영 편의(튜닝/리소스)
SLAM	ORB-SLAM3(Visual/Inertial)	높음	중간	높음	중간	중간
SLAM	LIO-SAM(LiDAR-Inertial)	매우 높음	중간	높음	높음	낮음(설정 복잡)
SLAM	Cartographer(2D/3D)	중간	높음	중간	중간	높음
Planner	RRT*	중간(준최적)	높음	해당 없음	중간	높음(구현 용이)
Planner	PRM*	중간	높음(오프라인 우수)	해당 없음	낮음(동적 취약)	중간
Planner	CHOMP/TrajOpt	높음(매끄러움)	중간	해당 없음	중간	낮음(미분·장애물 모델 필요)
Planner	MPC	높음(추종/제약)	중간	해당 없음	높음(예측)	낮음(모델·튜닝 난이도)

실제 성능은 센서 품질, 캘리브레이션, 하드웨어, 환경 복잡도에 따라 달라진다.

자율주행과 조작 작업에서의 적용

AMR/AGV는 창고 픽·패킹이나 동적 회랑에서 실시간 SLAM과 재계획을 함께 적용한다. 장애물이 추가되면 비용맵 갱신을 안전 정지 절차와 연동한다.

드론과 UGV 탐사에서는 GPS가 열악한 영역에서 LiDAR-Inertial SLAM과 전역 RRT*로 점검 루트를 생성할 수 있다. 이때 배터리 한계를 반영한 에너지 최소 경로가 필요하다.

매니퓰레이터는 카메라·심도센서 기반 TSDF 지도와 CHOMP/TrajOpt를 이용해 충돌 없는 곡면 궤적을 생성한다. 공정 사이에 랙과 부품 위치가 바뀌면 온라인 재계획으로 대응한다.

입력부터 실행 로그까지의 운영 흐름

입력은 LiDAR/Camera/IMU 센서 스트림, 로봇 키네마틱스, 목표·금지 영역으로 구성된다. 처리 흐름은 프런트엔드 정합, 그래프 최적화, 지도 생성, 전역 계획, 로컬 제어, 재계획 트리거 순으로 이어진다. 결과물은 정합된 지도와 포즈, 충돌 없는 경로·궤적, 제어 명령과 실행 로그다.

루프 클로저가 실패해 드리프트 임계를 넘으면 relocalization 모드로 전환하고 키프레임을 새로 초기화한다. 계획 실패나 타임아웃이 발생하면 비용을 상향하고 샘플을 늘리거나 해상도를 완화한 뒤 재시도한다. 이런 실패가 반복되면 안전 정지와 연산 예산 리셋으로 넘어간다. 센서가 드롭되면 IMU와 오도메트리로 관성 dead-reckoning을 유지하고, 신뢰도에 따라 가중치를 조정한다.

안전 여유 계수를 높이면 충돌 위험은 낮아지지만 경로는 더 보수적으로 변하고 시간·에너지 비용은 커진다. 지도 해상도를 높이면 정밀도는 올라가지만 메모리·CPU 사용량이 증가하고 실시간성은 떨어진다. 전역 저해상도와 로컬 고해상도를 병행하는 다중 해상도 구성이 이 균형에 적합하다. 온라인 최적화 빈도를 높이면 적응성은 커지지만 발열과 전력도 증가하므로 임무 우선순위 기반 스케줄링이 필요하다.

ROS 2와 OMPL로 실행하는 RRT* 예제

전제조건은 Ubuntu 22.04, ROS 2 Humble, Python 3.10이다. 필요한 패키지는 sudo apt install ros-humble-nav-msgs python3-ompl python3-numpy이며, 지도는 2D Occupancy Grid(0=자유, 1=장애물)를 가정한다.

아래 예제는 가상의 2D 격자 지도에서 시작점에서 목표점까지 RRT* 경로를 계획한다.

# file: simple_rrtstar.py
# 실행: python3 simple_rrtstar.py
import numpy as np
from ompl import base as ob
from ompl import geometric as og

W, H = 100, 100
grid = np.zeros((H, W), dtype=np.uint8)
grid[40:60, 50] = 1  # 세로 장애물

def is_free(x, y):
    xi, yi = int(x), int(y)
    if xi < 0 or yi < 0 or xi >= W or yi >= H:
        return False
    return grid[yi, xi] == 0

# 상태 공간 및 유효성 검사 설정
space = ob.RealVectorStateSpace(2)
bounds = ob.RealVectorBounds(2)
bounds.setLow(0); bounds.setHigh(0, W-1); bounds.setHigh(1, H-1)
space.setBounds(bounds)

ss = og.SimpleSetup(space)

def is_state_valid(si, state):
    return is_free(state[0], state[1])

ss.setStateValidityChecker(ob.StateValidityCheckerFn(lambda s: is_state_valid(ss.getSpaceInformation(), s)))

start = ob.State(space); start[0], start[1] = 10.0, 10.0
goal  = ob.State(space); goal[0],  goal[1]  = 90.0, 90.0
ss.setStartAndGoalStates(start, goal, 1.0)

planner = og.RRTstar(ss.getSpaceInformation())
planner.setRange(5.0)
ss.setPlanner(planner)

if ss.solve(1.0):  # 1초 시간 제한
    path = ss.getSolutionPath()
    path.interpolate(100)
    print("waypoints:", [(s[0], s[1]) for s in path.getStates()])
else:
    print("planning failed")

실제 로봇과 연동할 때는 /map(OccupancyGrid)을 구독한 뒤, is_free가 맵 해상도와 오리진에 맞게 변환하도록 보정한다. 동적 장애물은 시간 필드(cost inflation)로 처리하고, 실패하면 재계획 주기를 단축한다.

기대할 수 있는 변화

환경 복잡도가 중간인 조건에서는 경로 길이 10~~25% 단축, 목표 도달 시간 15~~30% 개선, 충돌/근접 이벤트 50% 이상 감소를 기대할 수 있다. 지도·포즈·경로의 일관성을 확보해 운영 안정성과 재현성을 높이고, 변경되는 환경에 대한 적응성도 키울 수 있다.

SLAM의 프런트엔드·백엔드 최적화와 모션 플래닝의 전역·로컬 계층을 분리하면서도 긴밀히 연결해야 한다. 재계획, 재지역화, 안전 페일세이프를 시스템 내부 절차로 두고, 운영 환경에 맞춰 센서 융합 전략·지도 표현·비용 함수·튜닝 체계를 설계하는 방식이 자율주행과 조작 시스템의 기반이 된다.