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 문제 정의
- 산업용 굴삭기는 건설, 농업, 광업, 임업 등에서 핵심 중장비이며, 자동화 추세가 가속화
- 경사면 굴착(excavation on a slope)은 슬립과 전복 위험이 증가하여 더욱 도전적
- 기존 연구의 한계:
- 평지 굴착에 초점 [5][6][7][8] → 슬립/전복 위험 미고려
- ZMP/마찰력 기반 안정성 연구 [10]-[16] → 굴착력(manipulation force)을 무시하거나 연속시간 토크 제어를 가정
- 산업용 굴삭기는 유압 구동(MCV) 기반으로 저수준 디지털 운동학적 제어(low-level digital kinematic control)만 가능 → 연속시간 토크 제어 불가
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)
- $\mu$와 $\xi$의 공통 분모 분수 구조를 활용하여 슬립/전복 방지 조건을 단일 선형 부등식으로 통합
- Transformer 기반 굴착력 차분 예측 모델 ($\Delta\hat{f}_{b_k}$) 개발
- Box uncertainty 기반 Robust Optimization (RO)으로 예측 오차 보상
- 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}$$
- $q_g$: DMP 목표점 (차체 좌표계 ${B}$ 기준 상수)
- $D, K$: 대각 상수 게인 행렬
- $f_m(s)$: 전문가 궤적을 모방하는 shaping force [5]
- $u$: 본 연구에서 설계하는 변조 입력
역기구학(inverse kinematics)으로 $q_b$에서 각 링크 좌표 $q_i$로의 사상이 유일하게 존재하며, 이를 미분하면:
$$\ddot{q}_i = J_i(q_b)u + n_i(q_b, \dot{q}_b) \tag{2}$$
- $J_i(q_b) \in \mathbb{R}^{3 \times 3}$: 야코비안 행렬
- $n_i(q_b, \dot{q}_b)$: 잔여 비선형 항
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}$$
- $\mu_s$: 정마찰계수
- $\bar{\mu}$: 안전 마진을 둔 한계값
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}$$
- $G_i = (-m_i y_i, \ m_i x_i, \ I_i) \in \mathbb{R}^3$
전복 방지 조건:
$$|\xi| < \bar{\xi} < \xi_s \tag{7}$$
- $\xi_s$: 하부 프레임 파라미터로 정의되는 안정 영역 크기
- $\bar{\xi}$: 안전 마진을 둔 한계값
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}$$
- $A(q_b) \in \mathbb{R}^{4 \times 3}$, $b(q_b, \dot{q}_b) \in \mathbb{R}^4$, $L(q_b) \in \mathbb{R}^{4 \times 2}$
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}}$, $\dot{q}{b{k+1}}$: Explicit Euler 적분으로 예측 가능
$$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}$$
- $f_{b_{k+1}}$: 토양 역학이 복잡하여 직접 예측 불가 → 데이터 기반 힘 차분 예측 필요
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$$
- $N_{k+1} \in \mathbb{R}^4$: $l_1$-norm 벡터 스택
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)
- 정마찰계수: 경사 ~12.8도에서 슬립 시작 → $\mu_s \approx 0.227$
- 실험 경사: $\psi_0 \in {-6.0°, -7.5°}$ (음의 경사면)
- 한계값: $\bar{\mu} \in {0.18, 0.16}$ (큰/작은 안전 마진)
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)
- 안정 영역: $\xi_s \approx 0.147$[m] (스윙 각도 $\frac{\pi}{2}$ 회전으로 축소)
- 실험 경사: $\psi_0 \in {30°, 35°}$ (양의 경사면)
- 한계값: $\bar{\xi} \in {0.12, 0.1}$[m]
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 굴착 확장 |
| 실물 굴삭기 미검증 (스케일다운만) | 빠르고 정확한 토양-버킷 상호작용 시뮬레이션 구축 |
| 슬립과 전복 동시 발생 시나리오 미검증 | 동시 위반 조건 실험 필요 |
한마디로 정리
- 경사면 굴착 중 슬립/전복 방지를 실시간 QP 변조로 구현
- $\mu$(슬립)와 $\xi$(전복)의 공통 분모 분수 구조를 발견하여 단일 선형 부등식으로 통합
- Transformer 기반 굴착력 차분 예측 모델로 디지털 제어의 인과성 문제 해결
- Robust Optimization + Box Uncertainty로 예측 오차에 대한 안전성 보장
- 스케일다운 굴삭기로 슬립/전복 방지 실험 성공적 검증 달성
- 실물 굴삭기, 3D 굴착, 다양한 토양 조건 등은 미대응
나의 의견
(작성 예정)
참고 코드
- GitHub: https://github.com/changukim-inrol/modulation_ral24.git