2025 Towards Fully Integrated Autonomous Excavation-서울대_요약

Towards Fully Integrated Autonomous Excavation - 요약


한줄 요약

30톤 실제 굴착기에서 지형 인식 → 굴착 경로 생성 → MPC 제어 → 정밀 지형 검사를 모두 온보드 센서만으로 수행하는 완전 통합 자율 굴착 시스템(AES) 을 제안하고, 45° 경사면 절토에서 cm 수준 정밀도를 달성했다.


1. 연구 배경 및 동기

문제

기존 연구의 한계

한계 설명
절토+검사 미통합 Zhang et al., Jelavic et al. 등은 절토만 수행, 사후 검사 미포함
센서 FOV 제한 캐빈 상단에만 라이다 배치 → 경사면 절토 시 ROI 미커버
물리적 제약 무시 기존 연구의 다수가 파워/유량 제약을 위반하거나 부분적으로만 고려
실기 검증 부재 물리적 제약을 고려한 연구도 시뮬레이션에서만 검증

본 연구의 기여

  1. 센서 구성: 어떤 경사에서도 ROI를 커버하는 듀얼 라이다 + 스테레오 카메라 배치
  2. 지형 추정: 칼만 필터형 MAP 추정으로 실시간 2.5D 높이맵 업데이트
  3. 모션 생성: 기하학적 + 물리적 제약을 모두 만족하는 2단계 모션 플래닝
  4. 정밀 검사: 2-Step Bundle Adjustment로 서브 cm 정밀도의 온보드 지형 검사

2. 시스템 구성

400 30톤 현대 HX300AL 자율 굴착기 외관 및 센서 배치 구성

전체 아키텍처

700 전체 시스템 아키텍처: 센서 입력 → 지형 추정 → 모션 생성 → MPC 제어 → 검사 파이프라인

graph TD
    L1[캐빈 라이다] --> LE[실시간 지형 추정]
    L2[붐 라이다] --> LE
    JA[관절 각도 센서] --> LE
    LE -->|2.5D 높이맵| PG[굴착 경로 생성]
    PG -->|기하학적 경로| MPC[MPC 경로 추적]
    PS[압력 센서] --> MPC
    MPC -->|실린더 제어| EXC[유압 굴착기]
    EXC -->|반복 사이클| LE
    SC[스테레오 카메라] --> INS[정밀 지형 검사]
    L1 --> INS
    L2 --> INS

2.1 센서 구성

700 듀얼 라이다(캐빈+붐)와 스테레오 카메라 배치, 각 센서의 FOV 커버리지

센서 위치 역할 정밀도
캐빈 라이다 캐빈 측면 (기울임) 원거리 일관 관측, 붐 움직임 무관 ~3cm
붐 라이다 붐 하단 (회전축 평행) 경사면 밀집 스캔, 붐과 함께 이동 ~3cm
스테레오 카메라 붐 하단 (라이다 옆) 정밀 검사용 고해상도 이미지 <1cm (BA 후)
IMU 각 관절 팔 자세 측정 0.1°
로터리 엔코더 스윙축 캐빈 회전각 2°
압력 센서 유압 실린더 내부 MPC 파워 제약 계산 -

핵심 아이디어: 붐 하단에 라이다를 장착하여 붐 회전축과 평행하게 배치 → 붐이 어떤 각도에 있든 경사면을 밀집 스캔 가능. 기존 연구(캐빈 상단만 배치)의 경사면 FOV 한계를 해결. 드론 같은 외부 장비 불필요.

2.2 센서 캘리브레이션 (4단계)

graph LR
    A[C2C<br>카메라↔카메라] --> B[L2C<br>라이다↔카메라]
    B --> C[L2L<br>라이다↔라이다]
    C --> D[S2V<br>센서↔굴착기]
    style A fill:#dbeafe
    style B fill:#fef3c7
    style C fill:#ede9fe
    style D fill:#d1fae5
  1. C2C (Camera-to-Camera): 스테레오 카메라 내부 파라미터 - Zhang 방법 [17]
  2. L2C (Lidar-to-Camera): 라이다-카메라 외부 파라미터 - 자동 타겟 검출 + 리프로젝션 에러 최소화 [18]
  3. L2L (Lidar-to-Lidar): 두 라이다 간 외부 파라미터 - 동시 체커보드 관측 [18]
  4. S2V (Sensors-to-Vehicle): 전체 센서 → 굴착기 좌표계 정렬 - 관절 각도 센서와 결합

3. 실시간 지형 추정

700 실시간 지형 추정: 라이다 포인트 클라우드 필터링 → 2.5D 높이맵 생성·업데이트 과정

3.1 2.5D 높이맵

작업 영역을 40cm × 40cm 격자로 분할하고, 각 셀 $c$에 높이의 기대값 $\hat{x}_c$과 분산 $\hat{\sigma}_c^2$을 관리한다. 전체 3D가 아닌 2.5D를 쓰는 이유는, 건설 지형은 각 수평 위치에 높이가 하나이므로 셀당 스칼라 높이면 충분하기 때문이다.

3.2 포인트 클라우드 필터링

굴착 중 버킷, 팔, 떨어지는 흙이 라이다에 잡히는 문제를 기구학 기반 필터링으로 해결한다.

graph TD
    P[라이다 점] --> Q{굴착기 부품<br>바운딩 박스 안?}
    Q -->|Yes| R[제거]
    Q -->|No| S[지면 점으로 사용]
    style R fill:#fee2e2
    style S fill:#d1fae5
# 포인트 클라우드 필터링 의사코드
for point in lidar_points:
    reject = False
    for part in [boom, stick, bucket]:
        bbox = compute_3D_bbox(part, current_joint_angles)
        if point_in_bbox(point, bbox):
            reject = True
            break
    if not reject:
        assign_to_cell(point, heightmap)

3.3 칼만 필터형 MAP 추정

지형 동역학 모델 (식 1):

지형의 높이가 시간에 따라 변할 수 있다고 가정하는 확률적 동역학 모델:

$$x_{k+1}^c = \begin{cases} x_{\text{bucket}}^c + u_k^c & \text{(버킷이 지면 아래로 통과한 경우)} \ x_k^c + w_k^c & \text{(그 외)} \end{cases}$$

기호 의미
$x_k^c$ 시각 $t_k$에서 셀 $c$의 높이
$x_{\text{bucket}}^c$ 버킷 팁이 지나간 높이 (굴착 시 지형이 깎임)
$u_k^c$ 굴착 후 노이즈 ~ $\mathcal{N}(0, Q_u)$
$w_k^c$ 자연 변화 노이즈 ~ $\mathcal{N}(0, Q_w)$ (미세 산사태 등)

직관: $Q_u$, $Q_w$가 작으면 "지형이 잘 안 변한다"고 가정 → 이전 추정값을 더 신뢰. $Q_u$, $Q_w$가 크면 지형 변화가 잦다고 가정 → 최신 센서 측정값에 더 의존.

측정 모델 (식 2):

$$y \sim \mathcal{N}!\left(x_k^c, \; v + C \cdot d^2\right)$$

기호 의미
$y$ 라이다 높이 측정값
$x_k^c$ 셀 $c$의 실제 높이 (추정 대상)
$v \geq 0$ 라이다 거리 측정 분산 (센서 스펙에서 제공)
$C \geq 0$ 지형 최대 기울기 관련 파라미터
$d$ 라이다 점에서 셀 중심까지의 수평 거리

직관: 셀 중심에서 멀리 떨어진 점일수록 해당 셀의 높이를 대표하기 어렵다 ($C \cdot d^2$ 항). 경사면에서는 40cm만 벗어나도 높이가 크게 달라질 수 있기 때문.

MAP 추정 과정:

각 셀에 높이의 기대값과 추정 분산을 함께 관리한다 (초기값: 분산 무한대, 기대값 0). 매 업데이트마다, 이전 시점 $t_k$의 추정값과 $t_k \sim t_{k+1}$ 사이에 들어온 라이다 점들을 결합하여 MAP(최대 사후 확률) 추정을 수행한다. 이렇게 구한 MAP 평균 $\hat{x}{k+1}^c$이 시각 $t{k+1}$에서의 지형 정보로 사용되어 충돌 회피 및 굴착 경로 생성에 활용된다.

📐 보충 수식 (논문에 명시되지 않은 유도 수식)

위 동역학(식 1) + 측정(식 2) 모델에 표준 칼만 필터 프레임워크를 적용하면, MAP 업데이트는 다음과 같이 유도된다:

$$\hat{x}_{k+1}^c = \frac{\frac{1}{\hat{\sigma}_k^2} \cdot \hat{x}_k + \sum_i w_i \cdot y_i}{\frac{1}{\hat{\sigma}_k^2} + \sum_i w_i}, \quad w_i = \frac{1}{v + C \cdot d_i^2}$$

이전 추정값과 새 측정값의 신뢰도(분산의 역수) 가중 평균으로, 셀 중심에 가까운 점($d_i$↓)일수록 가중치 $w_i$가 크다.

출처: 식 (1), (2)에 표준 칼만 필터 MAP 업데이트 규칙을 적용하여 유도

# 칼만 필터형 MAP 업데이트 의사코드
for cell in heightmap:
    # 예측 단계: 동역학 모델 (식 1) 적용
    if bucket_swept_cell(cell, t_k, t_k1):
        x_pred = bucket_tip_height(cell)
        var_pred = Q_u
    else:
        x_pred = cell.height      # 이전 추정값 유지
        var_pred = cell.variance + Q_w  # 불확실성 증가

    # 업데이트 단계: 새 라이다 측정값들로 MAP 추정
    for p in filtered_points_in_cell(cell):
        d = horizontal_distance(p, cell.center)
        R = v + C * d * d          # 측정 분산 (식 2)
        K = var_pred / (var_pred + R)  # 칼만 게인
        x_pred = x_pred + K * (p.z - x_pred)
        var_pred = (1 - K) * var_pred

    cell.height, cell.variance = x_pred, var_pred

3.4 성능 & 로버스트니스


4. 굴착 모션 생성

4.1 2단계 구조

기하학적 제약(위치 관련)과 물리적 제약(속도/힘 관련)을 분리하여 효율적으로 해결한다.

단계 목적 방법 제약 유형 시간
경로 생성 (Path) 기하학적 제약 만족 DFS + A* 충돌 회피, 버킷 자세 등 ~0.3s
경로 추적 (Trajectory) 물리적 제약 만족 MPC 파워, 유량, 변위 한계 <0.08s

4.2 5단계 굴착 사이클

400 5단계 굴착 사이클: 공격점 이동 → 굴착 → 들어올리기 → 덤핑 이동 → 덤핑

graph LR
    P1[Phase 1<br>이동→공격점] --> P2[Phase 2<br>굴착]
    P2 --> P3[Phase 3<br>들어올리기]
    P3 --> P4[Phase 4<br>덤핑 이동]
    P4 --> P5[Phase 5<br>덤핑]
    P5 -->|반복| P1
    style P1 fill:#fef3c7
    style P2 fill:#fee2e2
    style P3 fill:#ede9fe
    style P4 fill:#dbeafe
    style P5 fill:#d1fae5
Phase 동작 핵심 제약 알고리즘
1 초기 → 공격점 이동 충돌 회피 A* (4D: 스윙+3실린더)
2 굴착 수행 스윙 고정, 과굴착 방지, 용량 제한 DFS (3D: 3실린더)
3 버킷 들어올리기 충돌 회피 A* (4D)
4 덤핑 위치로 이동 흙 유출 방지 (버킷 방향 제약) A* (4D)
5 덤핑 - A* (4D)

4.3 두 가지 굴착 프로파일

graph TD
    START[굴착 시작] --> ND[Normal Digging<br>최대 부피 굴착]
    ND --> CHECK{높이 오차 ≤ 50cm?}
    CHECK -->|No| ND
    CHECK -->|Yes| TG[Tip Grading<br>정밀 표면 마감]
    TG --> DONE[완료]
    style ND fill:#fee2e2
    style TG fill:#d1fae5

400 일반 굴착(위)과 팁 그레이딩(아래) 경로 프로파일 비교: 최대 부피 굴착 vs 정밀 표면 마감

일반 굴착 (Normal Digging)

# DFS 기반 일반 굴착 경로 탐색 의사코드
def find_digging_path(landscape, target):
    root = find_point_of_attack(landscape, target)  # 가장 먼 교점
    stack = [root]
    while stack:
        node = stack.pop()  # DFS: 후입선출
        if is_feasible(node):  # 용량, 절삭각, 관통 체크
            if excavated_volume(node) >= target_volume:
                return path_to(node)
        children = expand(node)  # 실린더 길이 한 스텝 변경
        children.sort(key=lambda n: heuristic(n))  # 부피↑ + 파워↓
        stack.extend(children)

400 팁 그레이딩 동작 상세: 목표 지형을 따라 버킷 팁으로 표면을 정밀하게 다듬는 과정

팁 그레이딩 (Tip Grading)

4.4 A* 경로 계획 (Phase 1, 3, 4, 5)


5. MPC 기반 경로 추적

5.1 왜 MPC가 필요한가

경로(Path)에는 시간 정보가 없다. "어디를 지나가라"만 있고 "얼마나 빨리"가 없다. MPC는 시간을 배분하면서 동시에 물리적 제약을 실시간으로 만족시킨다.

graph LR
    REF[참조 경로<br>q_r] --> MPC[MPC 최적화<br><0.08s]
    STATE[현재 상태<br>q_0] --> MPC
    PRESS[유압 압력<br>f_p] --> MPC
    MPC --> U[최적 제어 입력<br>u*]
    U --> EXC[굴착기]
    EXC -->|피드백| STATE
    style MPC fill:#ede9fe

5.2 동역학 모델 (식 3)

1차 이산 시간 기구학 모델: $$q_{k+1} = f_d(q_k, u_k)$$

변수 차원 의미
$q$ $\mathbb{R}^4$ configuration: 스윙 각도 + 3개 실린더 길이
$u$ $\mathbb{R}^4$ 제어 입력: 스윙 속도 + 3개 실린더 신축 속도
$f_d$ - 이산 시간 상태 전이 함수

기구학 관계: $\dot{q} = J(q) \cdot u$, 여기서 $J(q)$는 자코비안 행렬.

설계 선택: 고차 유압 동역학 모델(압력, 관성 포함)은 유온(oil temperature)/마찰 불확실성이 크고 비선형성이 강해 실시간 MPC 적용이 어렵다. 대신 1차 기구학 모델 + 충분한 성능의 저수준 제어기 조합이 실용적이며, 실험으로 유효성을 검증했다.

5.3 물리적 제약 (식 4)

$$c(q_k, u_k, f_p) \leq 0$$

기호 의미
$f_p$ 유압 실린더 내부 압력 측정값 (파워 제약 계산에 사용)
$c(\cdot)$ 물리적 제약 함수 (3가지 제약을 벡터로 결합)

논문에서 고려하는 3가지 물리적 제약: 1. 파워 한계 (Power limit) 2. 실린더 변위 한계 (Cylinder displacement limit) 3. 펌프 유량 한계 (Pump flow rate limit)

📐 보충 수식 (논문 [15]에 상세 정의, 본 논문에서는 $c(\cdot) \leq 0$으로만 기술)

제약 수식 물리적 의미
파워 한계 $\sum_i p_i \cdot \dot{l}i \leq P{max}$ 유압 펌프 최대 출력
실린더 변위 $l_{min} \leq l_i \leq l_{max}$ 각 실린더 스트로크 범위
펌프 유량 $\sum_i A_i \cdot \dot{l}_i

출처: 저자 이전 연구 [15]에서 상세 정의

5.4 MPC 최적화 문제 (식 5)

$$\min_{q_{(\cdot)}, \, u_{(\cdot)}} J(q_r, u; \, q_{(\cdot)})$$

제약 조건: | 제약 | 수식 | 의미 | |------|------|------| | 동역학 (식 3) | $q_{k+1} = f_d(q_k, u_k)$ | 물리 법칙 | | 초기 상태 | $q_0 = q(t)$ | 현재 위치 | | 물리적 제약 (식 4) | $c(q_k, u_k, f_p) \leq 0$ | 파워, 유량, 변위 한계 |

비용함수 $J$는 상태 오차 $q - q_r$과 제어 입력 $u$에 대한 이차 형식(quadratic)이며, Augmented Lagrangian 방법 기반 제약 iLQR [20]로 실시간 풀이한다.

📐 보충 수식 (논문에서 "quadratic with respect to $q - q_r$ and $u$"로만 기술)

$$J = \sum_{k=0}^{N-1} \left[ (q_k - q_{r,k})^\top Q \, (q_k - q_{r,k}) + u_k^\top R \, u_k \right]$$

출처: 표준 이차 비용함수(LQR) 형태로 유도

5.5 시간 배분


6. 정밀 지형 검사

500 멀티뷰 스테레오 검사: 60° 스윙 촬영 → 특징점 추적 → 2-Step BA로 서브 cm 정밀도 3D 복원

6.1 방법: 멀티뷰 스테레오 + 2-Step Bundle Adjustment

6.2 프로세스

graph TD
    A[60° 스윙 촬영<br>~60초] --> B[Affine KLT<br>특징점 추적]
    B --> C[Step 1: 소수 점 + 전체 포즈<br>동시 최적화]
    C --> D[Step 2: 포즈 고정<br>전체 점 최적화]
    D --> E[정밀 3D 점군<br>< 1cm 정밀도]
    style C fill:#fef3c7
    style D fill:#d1fae5
    style E fill:#dbeafe
  1. 촬영: 붐 고정 + 60° 스윙 → 연속 스테레오 이미지 촬영 (~60초)
  2. 특징점 추적: Constrained Affine KLT — 일반 KLT는 수직 관측을 가정하나, 굴착기 카메라는 비스듬히 보므로 아핀 변환으로 패치 변형을 모델링
  3. 3D 복원: 2-Step Bundle Adjustment

6.3 2-Step BA (핵심 혁신)

문제: Full BA는 모든 카메라 포즈(6DoF × N장)와 모든 3D 점(3 × M개)을 동시 최적화. M이 수백만이면 포즈-점 커플링 항으로 계산 폭발.

핵심 통찰: 카메라 포즈를 정확히 결정하는 데 수백만 점은 필요 없다. 수천 개면 충분하다.

Step 최적화 대상 점 개수 포즈 특징
1 포즈 + 점 동시 ~수천 (FAST score 상위) 자유 포즈 결정에 충분
2 점만 수백만 전체 고정 커플링 없음 → 빠름

정밀도 손실은 Full BA 대비 무시할 수 있는 수준이며, 계산 속도는 수십 배 향상.

6.4 라이다 사전 정보 활용

라이다 높이맵 → 특징점 3D 위치 초기값 제공 → BA 수렴 속도·안정성 향상. 라이다(대략적, 빠름) + 카메라(정밀, 느림)의 센서 퓨전 효과.

6.5 성능 비교

700 검사 정밀도 비교: 라이다 단독, 단일뷰 스테레오, 제안 방법(2-Step BA)의 3D 복원 결과

500 검사 방법별 정밀도 및 소요 시간 비교 (Trimble Total Station 대비)

500 2-Step BA vs Full BA 계산 시간 및 정밀도 비교

방법 정밀도 시간 (35m²) 속도 (s/m²) 비고
Trimble Total Station ~mm 1,192초 34.2 별도 장비+인력
3D 라이다 단독 ~3cm 수 초 <0.1 정밀도 부족
단일뷰 스테레오 ~2cm 수 초 <0.1 시야각 제한
제안 방법 (2-Step BA) <1cm 25.8초 0.74 온보드 자동

→ Trimble 대비 ~46배 빠르면서 유사한 정밀도


7. 실험 결과

700 45° 경사면 실제 자율 굴착 실험: 절토 전후 지형 변화 및 목표 대비 오차

실험 설정

항목 값
장비 Hyundai HX300AL (30톤), 버킷 1m³, 강철 이빨 10cm
작업 45° 상향 경사면 절토, 폭 3.5m × 높이 1.5m
방법 3개소에서 수평 이동하며 반복 굴착
제어 주파수 100Hz (전자유압 밸브)
온보드 컴퓨터 쿼드코어 1.8GHz, 8GB RAM (GPU 없음)

주요 결과

지표 결과 의미
절토 평균 오차 7.4cm (σ = 5.3cm) 전문 기사 수준
검사 정밀도 <1cm 건설 검수 기준 충족
검사 소요 시간 85.8초 (촬영 60 + 계산 25.8) Trimble 대비 46배 빠름

계산 시간

모듈 시간 비고
지형 추정 실시간 (5Hz) 라이다 10Hz 입력
경로 생성 ~0.3초 덤핑-복귀 시간 내 완료 → 무중단 굴착
MPC <0.08초 실시간 물리적 제약 대응
검사 알고리즘 25.8초 (0.74 s/m²) Trimble 대비 46배 빠름

MPC 제약 만족


8. 한계 및 향후 연구

현재 한계

향후 연구 방향

graph TD
    subgraph 한계
        L1[야간/악천후]
        L2[지하 장애물]
        L3[단일 굴착기]
        L4[크롤러 이동 없음]
    end
    subgraph 향후
        F1[백업 센서<br>열화상 등]
        F2[유압 압력 기반<br>지하 3D 맵]
        F3[다중 굴착기<br>협업]
        F4[상위 태스크<br>플래닝]
    end
    L1 --> F1
    L2 --> F2
    L3 --> F3
    L4 --> F4

한마디로 정리


핵심 키워드

자율 굴착 AES MPC 번들 조정 2.5D 높이맵 칼만 필터 DFS A* KLT 멀티뷰 스테레오 유압 굴착기 온보드 센서 지형 검사


나의 의견

이 논문을 읽고 느낀 점, 우리 연구와의 연관성, 활용 가능성 등을 자유롭게 기록