2025 Real-Time_Excavation_Trajectory_Modulation_for_Slip_and_Rollover_Prevention-서울대(이동준)_요약

Real-Time Excavation Trajectory Modulation for Slip and Rollover Prevention

한줄 요약

경사면에서 자율 굴삭기의 슬립(slip)과 전복(rollover)을 실시간으로 방지하기 위해, 경사면 접선/법선 힘 비율 $\mu$와 영모멘트점(ZMP) $\xi$를 단일 선형 부등식으로 통합하고, Transformer 기반 굴착력 차분 예측 모델과 Robust Optimization을 결합한 QP 기반 궤적 변조 프레임워크를 제안한다.


1. 연구 배경 및 동기

1.1 문제 정의

1.2 핵심 도전 과제

도전 과제 설명
유압 구동 한계 MCV 기반 actuator는 포화, 히스테리시스, 데드존 등 복잡한 비선형 현상 → 토크 레벨 제어 불가
인과성 문제 (Causality-like issue) 현재 시점 $t_k$에서 다음 시점 $t_{k+1}$의 변조 $u_{k+1}$을 계산해야 하나, $f_{b_{k+1}}$은 알 수 없음
토양 역학 복잡성 기존 토양 모델은 파라미터 추정 및 계산량 문제로 실시간 적용 불가 [19][20][21]

1.3 기여 (Contribution)

  1. $\mu$와 $\xi$의 공통 분모 분수 구조를 활용하여 슬립/전복 방지 조건을 단일 선형 부등식으로 통합
  2. Transformer 기반 굴착력 차분 예측 모델 ($\Delta\hat{f}_{b_k}$) 개발
  3. Box uncertainty 기반 Robust Optimization (RO)으로 예측 오차 보상
  4. QP 형태의 실시간 최적화 → 스케일다운 굴삭기로 실험 검증

2. 시스템 구성 및 좌표계

400 Fig. 1. 경사면에서 굴삭기 구성: $_0$=베이스, $_1$=붐, $_2$=암, $_3$=버킷. 관성 좌표계 ${O}$, 차체 좌표계 ${B}$, 경사각 $\psi_0$, ZMP $\xi$, 안정 영역 $\xi_s$, 굴착력 $f_b$, 궤적 면적 $a_q$ 등이 표시됨.

주요 변수 정리

기호 의미
$q_b = (x_b, y_b, \phi_b)$ 버킷 끝단(end-tip) 좌표
$q_i = (x_i, y_i, \phi_i)$ $i$-번째 링크 CoM 위치 및 방향 ($i \in {1,2,3}$)
$\psi_0$ 경사면 각도 (상수)
$f_b = (f_{bx}, f_{by})$ 버킷 끝단에 작용하는 유효 굴착력
$f_o = (f_{ox}, f_{oy})$ 경사면 접선/법선 반력
$\mu$ 경사면 접선/법선 힘 비율 (슬립 지표)
$\xi$ 영모멘트점 ZMP 위치 (전복 지표)
$u \in \mathbb{R}^3$ DMP 궤적 변조 입력

3. 방법론

3.1 DMP 기반 공칭 궤적 (Nominal Trajectory)

DMP(Dynamic Movement Primitive) 기반 2차 미분방정식으로 정의:

$$\ddot{q}_b + D\dot{q}_b + K(q_b - q_g) = f_m(s) + u \tag{1}$$

역기구학(inverse kinematics)으로 $q_b$에서 각 링크 좌표 $q_i$로의 사상이 유일하게 존재하며, 이를 미분하면:

$$\ddot{q}_i = J_i(q_b)u + n_i(q_b, \dot{q}_b) \tag{2}$$

3.2 슬립 지표: 경사면 접선/법선 힘 비율 $\mu$

D'Alembert 원리로 각 축 반력을 계산:

$$f_{o\star} = \sum_i m_i(\ddot{\star}i - g\star) - f_{b\star}, \quad \star \in {x, y} \tag{3}$$

슬립 지표 $\mu$는:

$$\mu = \frac{\sum_i m_i(\ddot{x}i - g_x) - f{bx}}{\sum_i m_i(\ddot{y}i - g_y) - f{by}} \tag{4}$$

슬립 방지 조건:

$$|\mu| < \bar{\mu} < \mu_s \tag{5}$$

3.3 전복 지표: ZMP $\xi$

D'Alembert 원리로 ZMP를 유도:

$$\xi = \frac{\sum_i G_i \ddot{q}i + \sum_i m_i(y_i g_x - x_i g_y) + y_b f{bx} - x_b f_{by}}{\sum_i m_i(\ddot{y}i - g_y) - f{by}} \tag{6}$$

전복 방지 조건:

$$|\xi| < \bar{\xi} < \xi_s \tag{7}$$

3.4 $\mu$와 $\xi$의 공통 분수 구조

핵심 관찰: $\mu$ (식 4)와 $\xi$ (식 6) 모두 동일한 분모 $\sum_i m_i(\ddot{y}i - g_y) - f{by}$를 공유하며, 분자는 가속도 $\ddot{q}_i$에 대해 선형이다.

경사각 $\psi_0 \in [-\frac{\pi}{2}, \frac{\pi}{2}]$에서 $g_y < 0$이고, 링크 중력이 지배적이므로 분모를 양(positive)으로 가정:

$$\sum_i m_i(\ddot{y}i - g_y) - f{by} > 0 \tag{8}$$

flowchart TD
    A["μ = Numerator_μ / Common Denominator"] --> C["공통 분모 양수 가정 (식 8)"]
    B["ξ = Numerator_ξ / Common Denominator"] --> C
    C --> D["분모 양수 → 분수 부등식을 선형 부등식으로 변환"]
    D --> E["슬립 + 전복 방지 조건 → 단일 선형 부등식 (식 11)"]
    E --> F["A(q_b)u + b(q_b, q̇_b) + L(q_b)f_b ≤ 0"]

3.5 선형 부등식 제약 조건

$\mu$와 $\xi$를 변조 입력 $u$의 함수로 다시 표현:

$$\mu = \frac{A_\mu u + b_\mu - f_{bx}}{Cu + d - f_{by}}, \quad \xi = \frac{A_\xi u + b_\xi + y_b f_{bx} - x_b f_{by}}{Cu + d - f_{by}} \tag{9}$$

여기서: - $A_\mu = \sum_i m_i e_x \cdot J_i$, $\quad b_\mu = \sum_i m_i(e_x \cdot n_i - g_x)$ - $A_\xi = \sum_i G_i J_i$, $\quad b_\xi = \sum_i G_i n_i + \sum_i m_i(y_i g_x - x_i g_y)$ - $C = \sum_i m_i e_y \cdot J_i$, $\quad d = \sum_i m_i(e_y \cdot n_i - g_y)$

분모가 양이므로 슬립 및 전복 방지 조건 (5), (7)을 결합하여 단일 선형 부등식:

$$A(q_b)u + b(q_b, \dot{q}_b) + L(q_b)f_b \leq 0 \tag{11}$$

3.6 디지털 제어에서의 인과성 문제

저수준 디지털 운동학적 제어 채택으로 인해, 현재 시점 $t_k$에서 다음 시점 $t_{k+1}$의 부등식을 만족시켜야 함:

$$A(q_{b_{k+1}})u_{k+1} + b(q_{b_{k+1}}, \dot{q}{b{k+1}}) + L(q_{b_{k+1}})f_{b_{k+1}} \leq 0 \tag{12}$$

$$q_{b_{k+1}} = q_{b_k} + \dot{q}{b_k}\Delta t_k, \quad \dot{q}{b_{k+1}} = \dot{q}{b_k} + \ddot{q}{b_k}\Delta t_k \tag{13}$$

flowchart LR
    subgraph "시점 t_k (현재)"
        M["측정: q_bk, q̇_bk, f_bk"]
        P["예측: q_b(k+1), q̇_b(k+1) (Euler)"]
        F["예측: Δf̂_bk (Transformer)"]
    end
    subgraph "시점 t_(k+1) (다음)"
        U["u_(k+1) 적용"]
        S["슬립/전복 방지 조건 충족"]
    end
    M --> P
    M --> F
    P --> U
    F --> U
    U --> S

4. 핵심 기술: Transformer 기반 굴착력 차분 예측

4.1 모델 구조

400 Fig. 2. 데이터 기반 모델 $\Delta\hat{f}_{b_k}$의 구조. 인코더-디코더 Transformer 아키텍처 기반.

구성요소 세부사항
아키텍처 Attention-based Encoder-Decoder Transformer [22]
인코더 Multi-head self-attention + Feed-forward (잔차 연결 + Layer Norm)
디코더 Masked multi-head self-attention + Multi-head attention (인코더 출력 연결) + Feed-forward + Linear 출력
Attention heads 2
Feed-forward nodes 64
시간 윈도우 $w$ 10 (100Hz 기준)
인코더 입력 $x_k^{enc} \in \mathbb{R}^{9 \times w}$ 버킷 위치 $q_b$, 속도 $\dot{q}b$, 굴착력 $f_b$, 과거 궤적 면적 $a{q_b}$
디코더 입력 $x_k^{dec} \in \mathbb{R}^{2 \times w}$ $t_k$ 직전까지의 굴착력 차분
디코더 출력 $y_k^{dec} \in \mathbb{R}^{2 \times w}$ 굴착력 차분 예측 (마지막 원소가 $\Delta\hat{f}_{b_k}$)

4.2 학습 데이터 및 훈련

항목 값
총 데이터 수 $N \approx 1 \times 10^6$
굴착 궤적 수 470 (양/음 경사면 모두 포함)
데이터 분할 (학습:검증:테스트) 12:3:1
좌표계 관성 좌표계 ${O}$ (평탄 토양 기하 일관성)
프레임워크 TensorFlow + Adam optimizer
배치 크기 256
초기 학습률 0.001 (지수 감쇠 스케줄러)
손실 함수 MSE (Mean Square Error)

4.3 훈련 및 예측 결과

700 Fig. 3. $\Delta\hat{f}_{b_k}$ 훈련 및 예측 결과: (좌) 손실 함수 수렴 곡선, (중) 단일 테스트 궤적 예측 결과, (우) 예측 오차 분포. 실선은 box uncertainty 경계 $(\zeta_x, \zeta_y) = 0.05$[N], 점선은 3-sigma 및 5-sigma 신뢰 구간.

4.4 예측 오차와 Box Uncertainty

굴착력 예측은 오차 $\Delta e$를 포함:

$$f_{b_{k+1}} = f_{b_k} + \Delta\hat{f}_{b_k} + \Delta e \tag{14}$$

여기서 $\Delta e := \Delta f_{b_k} - \Delta\hat{f}_{b_k} \in \mathbb{R}^2$이며, box uncertainty로 모델링:

$${\Delta e \in \mathbb{R}^2 : |\Delta e_\star| \leq \zeta_\star, \quad \star \in {x, y}}$$

실험에서 $(\zeta_x, \zeta_y) = 0.05$[N]으로 설정.


5. Robust Optimization 기반 실시간 궤적 변조

5.1 불확실 선형 부등식

예측된 굴착력 (14)를 슬립/전복 조건 (12)에 대입:

$$A_{k+1}u_{k+1} + b_{k+1} + L_{k+1}(f_{b_k} + \Delta\hat{f}_{b_k} + \Delta e) \leq 0 \tag{15}$$

5.2 RO(Robust Optimization) 적용

$L_{k+1}\Delta e$를 행 벡터 $l_{k+1}^j \cdot \Delta e$로 분리하면, worst-case 상한:

$$l_{k+1}^j \cdot \Delta e \leq |l_{k+1}^j \cdot (\zeta_x, \zeta_y)|_1$$

따라서 (15)는 다음으로 변환:

$$A_{k+1}u_{k+1} + b_{k+1} + L_{k+1}(f_{b_k} + \Delta\hat{f}{b_k}) + N{k+1} \leq 0$$

5.3 QP 정식화

최종 최적화 문제:

$$\min_{u_{k+1}} \lambda_u |u_{k+1}|^2 + \lambda_h |u_{k+1} - u_k|^2 \tag{16}$$

$$\text{s.t.} \quad \underline{u}{k+1} \leq u{k+1} \leq \bar{u}_{k+1} \tag{17}$$

$$A_{k+1}u_{k+1} + b_{k+1} + L_{k+1}(f_{b_k} + \Delta\hat{f}{b_k}) + N{k+1} \leq 0 \tag{18}$$

비용 항 의미
$\lambda_u |u_{k+1}|^2$ 변조 크기 최소화
$\lambda_h |u_{k+1} - u_k|^2$ 변조 변화 smoothing (계층적 비용)
(17) 유압 구동 한계 (관절 속도/가속도 제한)
(18) 슬립/전복 방지 제약 (RO 포함)

이를 표준 QP 형태로 변환:

$$\min_{u_{k+1}} u_{k+1}^T P u_{k+1} + q \, u_{k+1} \quad \text{s.t.} \quad G u_{k+1} \leq h \tag{19}$$

→ 오픈소스 솔버(예: SciPy [24])로 실시간 해를 구함.

flowchart TD
    A["센서 측정<br/>q_bk, q̇_bk, f_bk"] --> B["Euler 적분<br/>q_b(k+1), q̇_b(k+1) 예측"]
    A --> C["Transformer 모델<br/>Δf̂_bk 예측"]
    B --> D["선형 부등식 구성<br/>A(k+1), b(k+1), L(k+1)"]
    C --> E["Box Uncertainty<br/>N(k+1) 계산"]
    D --> F["QP 정식화<br/>(식 16-18)"]
    E --> F
    F --> G["QP 솔버<br/>u(k+1) 계산"]
    G --> H["DMP 궤적 변조<br/>(식 1)에 u 주입"]
    H --> I["저수준 유압 제어기<br/>관절 속도 추종"]
    I --> J["t_(k+1)에서<br/>슬립/전복 방지 달성"]

6. 실험 결과

6.1 실험 환경

400 Fig. 4. 커스텀 스케일다운 굴삭기 실험 셋업: 토양 컨테이너, 경사면 조절 리니어 액추에이터, 센서 장착 구성.

항목 세부사항
굴삭기 LESU BA-B0001 (RC 유압 굴삭기) 커스텀 버전
유압 시스템 기어드 펌프 모터, 3-way 밸브, 실린더
조인트 센서 RLS 인코더 (200-250 Hz)
IMU PhidgetSpatial (방향 측정)
힘/토크 센서 Robotous F/T 센서 (프린트 브라켓 장착)
슬립 센서 VL53L4CD 레인지 센서 (200 Hz)
깊이 카메라 Intel Realsense D435 (30 Hz, 붐 링크 장착)
토양 규조토(diatomite)
컴퓨터 Intel NUC Mini PC @3.8GHz, 16GB RAM
OS/프레임워크 Ubuntu 20.04, Python + ROS
제어 주기 300 Hz (저수준), 100 Hz (궤적 변조 + 예측)
예측 계산 시간 $< 10^{-3}$ seconds

400 Fig. 5. 실험 시나리오 스냅샷: (좌) 슬립 방지 - 음의 6도 경사면, (우) 전복 방지 - 양의 35도 경사면. 전복 관찰을 위해 캐빈을 90도 회전하여 안정 영역을 의도적으로 축소.

6.2 시나리오 1: 슬립 방지 (Slip Prevention)

700 Fig. 6. 슬립 방지 실험 결과: (좌) $\psi_0 = -6°$, (우) $\psi_0 = -7.5°$. 버킷 끝단 궤적 $q_{b_x}, q_{b_y}$, 힘 비율 $\mu$, 슬립 변위 $d_x$, 굴착력 $f_{b_x}, f_{b_y}$.

주요 결과: - 변조 미적용 시: ~10초 부근에서 $\mu$가 $\mu_s$ 초과 → 최종 슬립 변위 약 0.147[m] - 변조 적용 시 ($\bar{\mu} = 0.18, 0.16$): $|\mu| < \mu_s$으로 성공적 규제 - 작은 $\bar{\mu}$일수록 변조 지속시간 길고, 얕은 굴착 궤적 생성 - 10회 반복 실험 결과: 변조 적용 시 슬립 변위 거의 0

6.3 시나리오 2: 전복 방지 (Rollover Prevention)

700 Fig. 7. 전복 방지 실험 결과: (좌) $\psi_0 = 35°$, (우) $\psi_0 = 30°$. 버킷 끝단 궤적, ZMP $\xi$, 피치각 $\psi$, 굴착력 $f_{b_x}, f_{b_y}$.

주요 결과: - 변조 미적용 시: ~2-5초에서 $\xi$가 $\xi_s$ 초과 → 큰 피치각(전복 지표) - 변조 적용 시 ($\bar{\xi} = 0.12, 0.1$): $\xi$가 안정 영역 내 유지 - 작은 $\bar{\xi}$일수록 양방향($x$, $y$) 모두 얕은 궤적, 작은 굴착력

6.4 통계 요약

400 Fig. 8. 10회 반복 실험 통계 요약: (좌) 슬립 변위 $d_x$, (우) 최대 피치각 $\psi$.

시나리오 조건 변조 미적용 $\bar{\mu}=0.18$ / $\bar{\xi}=0.12$ $\bar{\mu}=0.16$ / $\bar{\xi}=0.1$
Slip, $\psi_0=-6°$ 슬립 변위 $d_x$ 평균 0.127 [m] ~0 [m] ~0 [m]
Slip, $\psi_0=-7.5°$ 슬립 변위 $d_x$ 평균 0.133 [m] ~0 [m] ~0 [m]
Rollover, $\psi_0=35°$ 최대 피치각 $ \psi-\psi_0 $ (전복 발생)
Rollover, $\psi_0=30°$ 최대 피치각 $ \psi-\psi_0 $ 평균 1.64e-2 [rad]

7. 한계 및 향후 연구

한계 향후 과제
변조 $u$로 인한 비스무스(unsmooth) 궤적 궤적 스무스니스 보장 및 전역 최적성 연구
단일 토양 타입, 평탄 토양 표면만 고려 다양한 토양, 불규칙 표면 + 환경 센싱 확장
시상면(sagittal-plane) 2D 굴착만 가능 틸트로테이터 등 추가 부착물로 3D 굴착 확장
실물 굴삭기 미검증 (스케일다운만) 빠르고 정확한 토양-버킷 상호작용 시뮬레이션 구축
슬립과 전복 동시 발생 시나리오 미검증 동시 위반 조건 실험 필요

한마디로 정리


나의 의견

(작성 예정)


참고 코드