4족 보행 로봇 & 시뮬레이터

험지 환경 Locomotion 제어를 위한 첫 걸음

JungYeon Lee

2026-03-26

강의 로드맵

회차 주제
Lec 02 (오늘) 4족 보행 로봇 & IsaacGym 시뮬레이터
Lec 03 PPO 알고리즘 — 보행 정책 학습
Lec 04 VAE & DreamWaQ — 지형 인식 없이 걷기
Lec 05 Gazebo Sim2Sim — 배포 전 검증

최종 목표: 강화학습으로 훈련한 4족 로봇이 험지를 걷는 정책을 만들고, 시뮬레이터에서 검증

코드: go2_dreamwaq / quadruped_sim2sim

Part 1. 4족 보행 로봇이란?

왜 4족 보행 로봇인가?

바퀴 로봇의 한계

  • 평지에서만 이동 가능
  • 계단, 돌밭, 경사면 불가
  • 구조물 내부 탐색 어려움

4족 로봇의 장점

  • 험지 지형 돌파 가능
  • 자세 복원(recovery) 능력
  • 다양한 보행 패턴(gait)
  • Boston Dynamics Spot, Unitree Go1/Go2

대표적인 4족 로봇

로봇 제조사 무게 특징
Spot Boston Dynamics ~32kg 산업용, 고가
Go1 Unitree ~12kg 연구용, 저가
Go2 Unitree ~15kg Go1 후속, 성능 향상
ANYmal ANYbotics ~30kg ETH 출신, 산업 검사
A1 Unitree ~12kg 연구 커뮤니티에서 많이 사용

이 강의에서는 Go2를 메인 플랫폼으로 사용합니다. (Gazebo sim2sim은 Go1 URDF 기반)

로봇의 구조 — 링크 & 조인트

기본 구성

  • Base(몸체): IMU 센서 탑재
  • 다리 4개: 각 3개 관절 (총 12 자유도)
    • Hip (엉덩이) — 좌우 회전
    • Thigh (허벅지) — 앞뒤 회전
    • Calf (종아리) — 앞뒤 회전

12 자유도 (DoF) — IsaacGym 관절 순서

FL_hip,  FL_thigh,  FL_calf
FR_hip,  FR_thigh,  FR_calf
RL_hip,  RL_thigh,  RL_calf
RR_hip,  RR_thigh,  RR_calf

기본 자세 (Go2 default_dof_pos)

FL: hip=0.1,  thigh=0.8, calf=-1.5
FR: hip=-0.1, thigh=0.8, calf=-1.5
RL: hip=0.1,  thigh=1.0, calf=-1.5
RR: hip=-0.1, thigh=1.0, calf=-1.5

관측 공간 (Observation Space)

RL 에이전트가 매 스텝 받는 입력 정보 (base/waq/est task 기준):

관측값 차원 설명
base_ang_vel 3 IMU 각속도 (system delay 보간 적용)
projected_gravity 3 몸체 프레임 기준 중력 벡터
commands[:, :3] 3 \((v_x, v_y, \omega_{yaw})\) 목표 속도 명령
dof_pos - default_dof_pos 12 관절 각도 (기본 자세 대비 오프셋)
dof_vel 12 관절 각속도
actions 12 이전 스텝의 행동

총 관측 차원: \(3 + 3 + 3 + 12 + 12 + 12 = 45\)

Tip

Oracle task는 base_lin_vel 3차원을 추가로 포함하여 48차원입니다.

특권 관측 (Privileged Observations)

시뮬레이터에서만 접근 가능한 정보 — Critic 학습에만 사용:

특권 정보 차원 설명
disturb_force 3 외부 교란 힘
heights 187 발 주변 높이맵 (17×11 그리드, 0.8m × 0.5m)
합계 190
# legged_robot.py — 높이맵 측정
measured_points: 17x11 grid  # 로봇 기준 0.8m x 0.5m 범위
horizontal_scale = 0.05      # 5cm 간격
vertical_scale = 0.005       # 높이 해상도

학습 시 Critic은 obs(45) + est_vel(3) + privileged(190) = 238차원 입력을 사용

행동 공간 (Action Space)

RL 에이전트의 출력:

  • 12차원 연속값 — 각 관절의 목표 위치 오프셋
  • action_scale = 0.25, Hip 관절은 추가로 × 0.5

\[ q_{\text{target}} = q_{\text{default}} + a \times 0.25 \]

실제 토크 변환은 PD 제어기가 수행:

\[ \tau = K_p (q_{\text{target}} - q_{\text{current}}) - K_d \dot{q}_{\text{current}} \]

파라미터 값 (Go2)
\(K_p\) 20.0
\(K_d\) 0.5

보행 패턴 (Gait)

대표적인 gait 종류

  • Trot: 대각선 다리 쌍이 동시에 이동 (가장 안정적)
  • Walk: 한 번에 한 다리씩 (느리지만 안정)
  • Bound: 앞 두 다리, 뒤 두 다리 교대 (빠름)
  • Gallop: 비대칭 고속 보행

Trot Gait 타이밍

FL ████░░░░████░░░░
FR ░░░░████░░░░████
RL ░░░░████░░░░████
RR ████░░░░████░░░░

█ = stance (지면 접촉)
░ = swing  (공중)

DreamWaQ에서는 gait를 명시하지 않고 보상 함수로 자연스럽게 유도합니다.

Part 2. IsaacGym 시뮬레이터

왜 IsaacGym인가?

기존 시뮬레이터의 문제

  • MuJoCo, PyBullet: CPU 기반
  • 환경 수천 개 병렬화 어려움
  • 학습에 수일~수주 소요

IsaacGym의 장점

  • GPU 기반 물리 시뮬레이션 (PhysX)
  • 수천 개 환경 동시 실행
  • 텐서 데이터 GPU에서 바로 정책으로 전달
  • 학습 시간 대폭 단축 (수시간)

IsaacGym은 NVIDIA에서 개발한 GPU 가속 물리 시뮬레이터로, RL 학습에 최적화되어 있습니다.

IsaacGym 아키텍처

┌─────────────────────────────────────────────┐
│                    GPU                       │
│  ┌─────────────┐    ┌────────────────────┐  │
│  │  Physics Sim │───▶│  Observation 수집   │  │
│  │  (PhysX)     │    │  (GPU Tensor)      │  │
│  └──────▲───────┘    └────────┬───────────┘  │
│         │                     │              │
│         │                     ▼              │
│  ┌──────┴───────┐    ┌────────────────────┐  │
│  │  Action 적용  │◀───│  RL Policy (PPO)   │  │
│  │  (PD → 토크)  │    │  (PyTorch)         │  │
│  └──────────────┘    └────────────────────┘  │
└─────────────────────────────────────────────┘
  • 모든 연산이 GPU에서 수행 → CPU-GPU 데이터 전송 없음
  • torch.Tensor로 관측/행동 직접 주고받음

시뮬레이션 파라미터

go2_dreamwaq에서 사용하는 실제 설정값:

# base_config.py — 시뮬레이션 설정
dt = 0.005              # 물리 시뮬레이션 200 Hz
decimation = 4          # 정책 실행은 4스텝마다 → 50 Hz
substeps = 1
gravity = [0, 0, -9.81]

# 환경 설정
num_envs = 4096         # 병렬 환경 수
episode_length_s = 20   # 에피소드 길이 (초)
max_iterations = 5000   # Go2 기준
파라미터 의미
물리 dt 0.005s 200Hz 물리 시뮬레이션
decimation 4 정책은 50Hz (0.02s 간격)
num_envs 4096 동시 실행 환경 수
episode 20s 한 에피소드 = 1000 정책 스텝

프로젝트 구조

go2_dreamwaq/dreamwaq/
├── legged_gym/legged_gym/
│   ├── envs/
│   │   ├── __init__.py              # 태스크 등록 (go2_base, go2_oracle, go2_waq, go2_est)
│   │   ├── a1/a1_config.py          # A1 설정 (Base/Oracle/Waq/Est)
│   │   ├── go2/go2_config.py        # Go2 설정
│   │   └── base/
│   │       ├── legged_robot.py      # 메인 환경 클래스
│   │       └── legged_robot_config.py  # LeggedRobotCfg, LeggedRobotCfgPPO
│   ├── scripts/
│   │   ├── train.py                 # 학습 진입점
│   │   └── play.py                  # 추론/시각화
│   └── utils/
│       ├── terrain.py               # 지형 생성
│       └── task_registry.py         # TaskRegistry
└── rsl_rl/rsl_rl/
    ├── algorithms/ppo.py            # PPO 알고리즘
    ├── modules/actor_critic.py      # ActorCritic 네트워크
    ├── runners/on_policy_runner.py   # OnPolicyRunner, OnPolicyRunnerWAQ
    ├── storage/rollout_storage.py   # RolloutStorage (GAE)
    └── vae/cenet.py                 # CENet (DreamWaQ 핵심)

태스크 종류

태스크 Runner Actor 입력 Critic 입력 추가 네트워크
go2_base OnPolicyRunner 45 45 없음
go2_oracle OnPolicyRunner 238 238 없음
go2_waq OnPolicyRunnerWAQ 64 238 CENet
go2_est OnPolicyRunnerEst 48 238 EstNet
  • base: 기본 관측만 사용 (baseline)
  • oracle: 특권 정보를 직접 사용 (상한선 확인용)
  • waq: DreamWaQ — CENet으로 implicit terrain estimation
  • est: EstNet baseline (VAE 없이 속도만 추정)

환경 클래스 — compute_observations

# legged_robot.py — 관측값 구성
def compute_observations(self):
    self.obs_buf = torch.cat([
        self.base_ang_vel * self.obs_scales.ang_vel,     # [N, 3]
        self.projected_gravity,                           # [N, 3]
        self.commands[:, :3] * self.commands_scale,       # [N, 3]
        (self.dof_pos - self.default_dof_pos)
            * self.obs_scales.dof_pos,                    # [N, 12]
        self.dof_vel * self.obs_scales.dof_vel,           # [N, 12]
        self.actions,                                     # [N, 12]
    ], dim=-1)  # shape: [num_envs, 45]

    # 특권 관측 (Critic용)
    self.privileged_obs_buf = torch.cat([
        self.disturb_force,                               # [N, 3]
        self.measured_heights,                             # [N, 187]
    ], dim=-1)  # shape: [num_envs, 190]

    # 관측 히스토리 (CENet용)
    self.obs_history_buf = torch.roll(self.obs_history_buf, shifts=-1, dims=1)
    self.obs_history_buf[:, -1] = self.obs_buf
    # shape: [num_envs, 5, 45] → flatten → [num_envs, 225]

지형 생성 (Terrain)

# terrain.py — 지형 설정
terrain_proportions = [0.1, 0.1, 0.35, 0.25, 0.2]
# [smooth slope, rough slope, stairs up, stairs down, discrete obstacles]

mesh_type = 'trimesh'
horizontal_scale = 0.05   # 5cm 해상도
vertical_scale = 0.005    # 높이 해상도

num_rows = 10              # 난이도 레벨 (curriculum)
num_cols = 20              # 지형 종류
terrain_length = 8.0       # 각 블록 8m
terrain_width = 8.0
# 높이 측정 포인트 (로봇 주변)
measured_points_x: 17 points  # 로봇 기준 전후 0.8m
measured_points_y: 11 points  # 로봇 기준 좌우 0.5m
# 총 17 × 11 = 187 높이 샘플 → privileged_obs에 포함

Part 3. 보상 함수 설계

보상 함수가 중요한 이유

RL에서 보상은 “어떤 행동이 좋은가” 를 정의합니다.

보상이 잘못되면

  • 로봇이 넘어지면서 앞으로 감
  • 다리를 떨면서 제자리 진동
  • 비현실적인 동작으로 높은 보상 획득

보상이 잘 설계되면

  • 자연스러운 trot gait 학습
  • 명령 속도 정확히 추종
  • 에너지 효율적인 보행

보상 항목 (go2_dreamwaq 실제 설정)

보상 항목 가중치 수식
tracking_lin_vel 1.0 \(\exp(-\|v_{xy} - v_{xy}^{cmd}\|^2 / 0.25)\)
tracking_ang_vel 0.5 \(\exp(-(\omega_{yaw} - \omega_{yaw}^{cmd})^2 / 0.25)\)
lin_vel_z -2.0 \(v_z^2\)
ang_vel_xy -0.05 \(\|\omega_{xy}\|^2\)
orientation -0.2 \(\|g_{xy}^{proj}\|^2\) (기울기 페널티)
base_height -1.0 \((h - h_{target})^2\)
dof_acc -2.5e-7 \(\|\ddot{q}\|^2\)
joint_power -2.0e-5 \(\sum\|\tau \cdot \dot{q}\|\)
action_rate -0.01 \(\|a_t - a_{t-1}\|^2\)
smoothness -0.01 \(\|a_t - 2a_{t-1} + a_{t-2}\|^2\)
foot_clearance -0.01 \(\sum(h_{foot} - 0.12)^2 \cdot v_{lateral}\)

모든 보상에 dt = 0.02s (decimation × sim_dt)가 곱해집니다.

보상 함수 코드

# legged_robot.py — 보상 계산
def _reward_tracking_lin_vel(self):
    """선속도 추종 — 명령 속도에 가까울수록 높은 보상"""
    lin_vel_error = torch.sum(
        torch.square(self.commands[:, :2] - self.base_lin_vel[:, :2]), dim=1)
    return torch.exp(-lin_vel_error / self.cfg.rewards.tracking_sigma)  # σ=0.25

def _reward_tracking_ang_vel(self):
    """각속도 추종"""
    ang_vel_error = torch.square(
        self.commands[:, 2] - self.base_ang_vel[:, 2])
    return torch.exp(-ang_vel_error / self.cfg.rewards.tracking_sigma)

def _reward_lin_vel_z(self):
    return torch.square(self.base_lin_vel[:, 2])

def _reward_ang_vel_xy(self):
    return torch.sum(torch.square(self.base_ang_vel[:, :2]), dim=1)

def _reward_orientation(self):
    return torch.sum(torch.square(self.projected_gravity[:, :2]), dim=1)

def _reward_base_height(self):
    base_height = self.root_states[:, 2] - self.measured_heights.mean(dim=1)
    return torch.square(base_height - self.cfg.rewards.base_height_target)
    # Go2: target=0.3m

def _reward_joint_power(self):
    return torch.sum(torch.abs(self.torques * self.dof_vel), dim=1)

def _reward_smoothness(self):
    return torch.sum(torch.square(
        self.actions - 2 * self.last_actions + self.last_last_actions), dim=1)

Part 4. 정리 & 다음 강의

오늘 배운 것

  1. 4족 보행 로봇 구조 — 12 자유도, Go2/A1 기본 자세
  2. 관측/행동 공간 — 45차원 입력, 12차원 출력, PD 제어기
  3. IsaacGym — GPU 병렬 시뮬레이션, 4096개 환경, 50Hz 정책
  4. 프로젝트 구조 — base/oracle/waq/est 4가지 태스크
  5. 보상 함수 — 12개 항목, 추종 보상 + 페널티 가중합

다음 강의 — PPO 알고리즘

다룰 내용

  • Policy Gradient 기초
  • PPO (Proximal Policy Optimization)
  • 클리핑, GAE, Adaptive LR
  • rsl_rl/algorithms/ppo.py 코드 분석

준비물

  • PyTorch 기초 이해
  • go2_dreamwaq 레포 클론
  • GPU 사용 가능한 환경

참고 자료

Q & A

질문 있으신가요?