(한민확인)2021 Real-time motion planning of a hydraulic excavator using trajectory optimization and model predictive control - 서울대_요약

[!info] 관련 영상 📺 YouTube - Real-time Motion Planning of a Hydraulic Excavator

Real-Time Motion Planning of a Hydraulic Excavator using Trajectory Optimization and MPC

한줄 요약

유압 굴착기의 실시간 궤적 계획을 위해 global planner (trajectory optimization) 와 local planner (MPC + disturbance estimation) 를 통합한 프레임워크를 제안하며, force/torque, power, cylinder displacement, flow rate 등 물리적 제약조건을 모두 만족하면서 30 Hz 이상의 실시간 궤적 재계획이 가능함을 시뮬레이션으로 검증하였다.


1. 연구 배경

1.1 문제 정의

1.2 기존 연구 분류

분류 대표 연구 특징 한계
Trajectory optimization [6,7,9] 비용함수+제약조건+시스템 동역학 기반 전체 궤적 최적화 Offline 계산, 외란 미대응, 단순 커널 파라미터화
Receding horizon / MPC [12,13] 이동 수평선 방식 궤적 최적화 [12] 단일 입력 decoupled 동역학만 고려, [13] 연산시간 부족 (~3.3 Hz)
Non-optimization 기반 [1,14-16] DMP, reachability map, 계층적 제어 제약조건 명시적 고려 불가, feasibility 보장 어려움

1.3 연구 기여 (Contributions)

  1. Global + Local planner 통합: operational constraints와 physical constraints를 모두 고려
  2. 축소 차원 global trajectory optimization: 수 초 이내 계산 가능한 문제 정식화
  3. 실시간 제약 궤적 생성: 외란 추정 기반 disturbance-aware 실시간 궤적 계획

2. 시스템 및 방법론

2.1 전체 시스템 아키텍처

400 Fig. 2: 알고리즘 전체 흐름도. Global planner가 환경 모델 기반으로 전역 궤적 $\mathbf{x}_r$을 생성하고, Local planner (MPC + 외란 추정)가 이를 실시간 추종하며, Motion Controller가 제어 입력 $\mathbf{u}$를 PLANT에 인가한다. 점선은 피드백 신호를 나타냄.

flowchart LR
    ENV["환경 모델<br/>(지면 형상)"] --> GP["Global Planner<br/>(Trajectory Optimization)"]
    GP -- "x_r (전역 궤적)" --> LP["Local Planner"]

    subgraph LP_box["Local Planning"]
        DE["Disturbance<br/>Estimation"] --> MPC["Model Predictive<br/>Control"]
    end

    LP -- "x_d, u_d" --> MC["Motion Controller<br/>(FB Linearization PID)"]
    MC -- "u (제어입력)" --> PLANT["PLANT<br/>(굴착기 동역학)"]
    PLANT -- "x (상태)" --> LP
    PLANT -- "x" --> DE

2.2 굴착기 구성 (Hyundai HX300L)

400 Fig. 1: Hyundai HX300L 굴착기 구성. 4개의 회전 강체로 구성: (1) Cabin, (2) Boom, (3) Arm, (4) Bucket. 관절 각도 $\mathbf{q}_\theta$와 실린더 변위 $\mathbf{q}_L$로 configuration 정의 가능.


3. 핵심 기술

3.1 운동학 (Kinematics)

Configuration을 관절 각도 또는 실린더 변위로 표현:

$$ \mathbf{q}_\theta = [\psi_U \; \theta_B \; \theta_A \; \theta_K]^\top \in \mathbb{R}^4, \quad \mathbf{q}_L = [\psi_U \; L_B \; L_A \; L_K]^\top \in \mathbb{R}^4 $$

두 표현 사이의 비선형 전단사(bijective) 매핑:

$$ \mathbf{q}\theta = g_L(\mathbf{q}_L), \quad \mathbf{q}_L = g\theta(\mathbf{q}\theta), \quad \dot{\mathbf{q}}\theta = J_L(\mathbf{q}L)\dot{\mathbf{q}}_L, \quad \dot{\mathbf{q}}_L = J\theta(\mathbf{q}\theta)\dot{\mathbf{q}}\theta \tag{1} $$

버킷 팁 자세 (bucket tip pose):

$$ \mathbf{E} = [E_x(\mathbf{L}) \; E_z(\mathbf{L}) \; \theta(\mathbf{L})]^\top \in \mathbb{R}^3 $$

여기서 $\mathbf{L} = [L_B \; L_A \; L_K]^\top$, $\theta = \theta_B + \theta_A + \theta_K$.

3.2 동역학 (Dynamics)

관절 각도 기반 동역학 (Euler-Lagrange):

$$ \dot{\mathbf{x}}\theta = \begin{bmatrix} \dot{\mathbf{q}}\theta \ \ddot{\mathbf{q}}\theta \end{bmatrix} = \begin{bmatrix} \dot{\mathbf{q}}\theta \ M_\theta^{-1}(-\mathbf{h}\theta + J\theta^\top \mathbf{u} + \boldsymbol{\Delta}) \end{bmatrix} =: f_\theta(\mathbf{x}_\theta, \mathbf{u}, \boldsymbol{\Delta}) \tag{2} $$

기호 의미
$\mathbf{x}\theta = [\mathbf{q}\theta^\top \; \dot{\mathbf{q}}_\theta^\top]^\top \in \mathbb{R}^8$ 시스템 상태
$\mathbf{u} = [T_U \; F_B \; F_A \; F_K]^\top \in \mathbb{R}^4$ 제어 입력 (swing torque + 유압 실린더 force)
$M_\theta(\mathbf{q}_\theta) \in \mathbb{R}^{4 \times 4}$ 질량 행렬
$\mathbf{h}\theta(\mathbf{q}\theta, \dot{\mathbf{q}}_\theta) \in \mathbb{R}^4$ Coriolis-원심력 + 중력 합성항
$\boldsymbol{\Delta} \in \mathbb{R}^4$ 총괄 외란 (lumped disturbance)

[!abstract] 수식 상세 전개 $M_\theta$와 $\mathbf{h}_\theta$의 원소별 전개, 변수 색깔 구분(측정/CAD/미지), FL과의 연결은 별도 노트 참조: 굴착기 동역학 M과 h 수식 해부

실린더 변위 기반 동역학 (local planning에서 사용):

$$ \dot{\mathbf{x}}_L = \begin{bmatrix} \dot{\mathbf{q}}_L \ \ddot{\mathbf{q}}_L \end{bmatrix} = \begin{bmatrix} \dot{\mathbf{q}}_L \ M_L^{-1}(-\mathbf{h}_L + \mathbf{u} + \boldsymbol{\Delta}_L) \end{bmatrix} =: f_L(\mathbf{x}_L, \mathbf{u}, \boldsymbol{\Delta}_L) \tag{3} $$

여기서 $M_L(\mathbf{q}_L) = J_L^\top \tilde{M}_L J_L$, $\mathbf{h}_L = J_L^\top(\tilde{M}_L \dot{J}_L \dot{\mathbf{q}}_L + \tilde{\mathbf{h}}_L)$, $\boldsymbol{\Delta}_L = J_L^\top \boldsymbol{\Delta}$.

3.3 Global Planning

굴착 작업은 3개 phase로 구성:

400 Fig. 3: 단일 굴착 작업의 3개 phase. Phase 1: 초기 위치에서 지표면 진입점으로 이동. Phase 2: 버킷이 토양을 관통(굴착). Phase 3: 굴착된 토양을 목표 위치(트럭 등)로 운반.

flowchart LR
    P1["Phase 1<br/>초기 위치 → 지표면 진입<br/>(Bernstein 파라미터화)"] --> P2["Phase 2<br/>토양 굴착<br/>(Waypoint 최적화)"]
    P2 --> P3["Phase 3<br/>토양 운반<br/>(Bernstein 파라미터화)"]

    style P1 fill:#e8f4fd
    style P2 fill:#fde8e8
    style P3 fill:#e8fde8

Phase 2: 굴착 궤적 최적화

$n_2 + 1$개 waypoint $\mathbf{L}0, \mathbf{L}_1, \cdots, \mathbf{L}{n_2}$로 이산화하고, 비용함수를 다음과 같이 정의:

$$ J_{\text{Phase2}} = w_d \sum_{k=1}^{n_2} |\mathbf{L}k - \mathbf{L}{k-1}|_W^2 - w_v V \tag{4} $$

항 의미
$w_d \sum |\mathbf{L}k - \mathbf{L}{k-1}|_W^2$ 유압 실린더 이동 거리 최소화 (에너지 비용)
$w_v V$ 굴착 체적 최대화

지표면과 목표 형상은 다항식으로 모델링:

$$ z_{\text{surf}} = a_0^{\text{surf}} + a_1^{\text{surf}}x + \cdots + a_{n_{\text{surf}}}^{\text{surf}} x^{n_{\text{surf}}} \tag{5} $$

$$ z_{\text{target}} = a_0^{\text{target}} + a_1^{\text{target}}x + \cdots + a_{n_{\text{target}}}^{\text{target}} x^{n_{\text{target}}} $$

굴착 체적 (swept volume, 사다리꼴 적분):

$$ V_{\text{swept}} = \int_{x_{n_2}}^{x_0} z_{\text{surf}}(x)\,dx - \frac{z_0 + 2z_1 + \cdots + 2z_{n_2-1} + z_{n_2}}{2n_2} \cdot (x_0 - x_{n_2}) \tag{6} $$

$$ V = \min{V_{\text{swept}},\; V_{\text{cap}}(\theta(\mathbf{L}_{n_2}))} $$

400 Fig. 5: (좌) Phase 2에서의 버킷 궤적 제약조건. 실선은 현재 지표면, 점선은 목표 형상. (우) 버킷 형상이 버킷 팁 궤적을 관통하지 않아야 함 (삼각형 충돌 박스). 버킷 회전 각도 $\theta$와 여유 각도 $\alpha$ 표시.

Phase 2 제약조건 요약:

# 제약조건 설명
1 위치 제약 버킷 팁이 관심 영역 내, 목표 표면 위, 지표면 아래에 위치
2 단조 증가 $\theta$ 버킷 회전각이 단조 증가해야 함 (에너지 효율)
3 양의 여유 각도 $\alpha > 0$ 버킷 팁 외 부분이 지면을 밀지 않도록
4 버킷 몸체 위치 버킷 전체가 팁 경로 위에 위치
5 $\theta < \pi$ 토양 흘림 방지 (Fig. 4 참조)

400 Fig. 4: 버킷 용량 $V_{\text{cap}}$과 회전 각도 $\theta$의 관계. $\theta \geq \pi$이면 손목 방향으로 토양이 흘러 장비 고장 위험.

Phases 1 & 3: Bernstein 다항식 파라미터화

Bernstein 다항식:

$$ p(s) = \sum_{k=0}^{n} \beta_k \binom{n}{k} s^k (1-s)^{n-k}, \quad s \in [0, 1] \tag{7} $$

Phases 1, 3의 최적화 문제:

$$ \min_{\beta_k, T} \int_0^T \frac{1}{2} \mathbf{u}^\top W_u \mathbf{u}\, dt \tag{8} $$

$$ \text{s.t.} \quad \mathbf{q}L^l \leq \beta_k \leq \mathbf{q}_L^u, \quad \dot{\mathbf{q}}_L^l \leq \frac{n \cdot (\beta{k+1} - \beta_k)}{T} \leq \dot{\mathbf{q}}_L^u $$

Phase 3 추가 제약 (토양 흘림 방지):

$$ \theta_0 \leq \theta(\beta_k) \leq \pi \tag{9} $$

3.4 Local Planning (MPC)

물리적 제약조건

$$ \mathbf{u}^l \leq \mathbf{u} \leq \mathbf{u}^u \tag{10a} $$

$$ p = \mathbf{u}^\top \dot{\mathbf{q}}_L \leq p^u \tag{10b} $$

$$ \mathbf{L}^l \leq \mathbf{L} \leq \mathbf{L}^u \tag{10c} $$

$$ f_i = A_i(\text{sgn}(\dot{\mathbf{q}}_L))^\top \dot{\mathbf{q}}_L^{\text{abs}} \leq f_i^u, \quad i = 1, 2 \tag{10d} $$

제약 물리적 의미
(10a) Force/torque limit 각 액추에이터의 힘/토크 상하한
(10b) Power limit 유압 펌프의 총 파워 상한 (모든 액추에이터가 동일 파워원 공유)
(10c) Cylinder displacement 실린더 변위의 물리적 상하한
(10d) Pump flow rate 2개 유압 펌프의 유량 제한

외란 추정 (Disturbance Estimation)

모멘텀 기반 외란 추정 (momentum-based disturbance estimation):

$$ \hat{\boldsymbol{\Delta}} = K_E \left( \mathbf{p} - \mathbf{p}(0) - \int_0^t \left( J_\theta^\top \mathbf{u} + C_\theta^\top \dot{\mathbf{q}}\theta - G\theta + \hat{\boldsymbol{\Delta}} \right) d\tau \right), \quad \mathbf{p} = M_\theta \dot{\mathbf{q}}_\theta \tag{11} $$

$\dot{M}\theta = C\theta + C_\theta^\top$ 성질을 활용하면, 실제 외란 $\boldsymbol{\Delta}$의 저역통과 필터(low-pass filter) 로 작동:

$$ \dot{\hat{\boldsymbol{\Delta}}} = K_E(\boldsymbol{\Delta} - \hat{\boldsymbol{\Delta}}) $$

실린더 변위 기반 추정값: $\hat{\boldsymbol{\Delta}}_L = J_L^\top \hat{\boldsymbol{\Delta}} = [\hat{\Delta}_U \; \hat{\Delta}_B \; \hat{\Delta}_A \; \hat{\Delta}_K]^\top$

MPC 문제 정식화

원래 비선형 MPC:

$$ \min_{\mathbf{u}k} |\mathbf{x}_N - \mathbf{x}{r,N}|P^2 + \sum{k=0}^{N-1} \left( |\mathbf{x}k - \mathbf{x}{r,k}|_Q^2 + |\mathbf{u}_k|_R^2 \right) \tag{12} $$

$$ \text{s.t.} \quad \mathbf{x}{k+1} = f_d(\mathbf{x}_k, \mathbf{u}_k, \hat{\boldsymbol{\Delta}}_k), \quad c{k,i}(\mathbf{x}_k, \mathbf{u}_k) \geq 0 $$

Feedback Linearization 기반 단순화

가상 입력 $\mathbf{v} \in \mathbb{R}^4$와 피드백 선형화 제어 법칙 도입:

$$ \mathbf{u} = \text{FL}(\mathbf{x}, \mathbf{v}) := \mathbf{h}_L + M_L \mathbf{v} - \hat{\boldsymbol{\Delta}}_L $$

이를 통해 비선형 동역학을 선형 동역학으로 변환:

$$ \dot{\mathbf{x}} = A_c \mathbf{x} + B_c \mathbf{v}, \quad A_c = \begin{bmatrix} 0_{4\times4} & I_4 \ 0_{4\times4} & 0_{4\times4} \end{bmatrix}, \; B_c = \begin{bmatrix} 0_{4\times4} \ I_4 \end{bmatrix} \tag{13} $$

최종 MPC 문제 (선형화된 동역학 기반):

$$ \min_{\mathbf{v}k} |\mathbf{x}_N - \mathbf{x}{r,N}|{\tilde{P}}^2 + \sum{k=0}^{N-1} \left( |\mathbf{x}k - \mathbf{x}{r,k}|{\tilde{Q}}^2 + |\mathbf{v}_k|{\tilde{R}}^2 \right) \tag{14} $$

$$ \text{s.t.} \quad \mathbf{x}{k+1} = A_d \mathbf{x}_k + B_d \mathbf{v}_k, \quad c{k,i}(\mathbf{x}_k, \text{FL}(\mathbf{x}_k, \mathbf{v}_k)) \geq 0 $$

flowchart TB
    NL["비선형 동역학 (3)<br/>f_L(x_L, u, Delta_L)"] -->|"Feedback Linearization<br/>u = h_L + M_L*v - Delta_L_hat"| LIN["선형 동역학 (13)<br/>x_dot = A_c*x + B_c*v"]
    LIN -->|"이산화"| DIS["이산 선형 동역학<br/>x_{k+1} = A_d*x_k + B_d*v_k"]
    DIS --> MPC_P["MPC 문제 (14)<br/>- 선형 동역학 → 빠른 계산<br/>- 물리적 제약조건 유지<br/>- DDP + Augmented Lagrangian"]

    DE["외란 추정 (11)<br/>모멘텀 기반<br/>저역통과 필터 특성"] --> MPC_P

    style NL fill:#ffcccc
    style LIN fill:#ccffcc
    style MPC_P fill:#cce5ff

4. 실험 결과

4.1 시뮬레이션 환경

항목 설정
플랫폼 Hyundai HX300L 모델 파라미터
구현 ROS + C++ (각 모듈 개별 노드)
외란 모델 Modified LuGre friction (실린더 마찰) + Modified FEE (토양-버킷 상호작용)
전역 최적화 ALGLIB (비선형 최적화 솔버)
MPC 솔버 DDP + Augmented Lagrangian [30]
MPC 파라미터 시간 수평선 1000 ms, 제어 이산화/적분 주기 20 ms
Bernstein 제어점 Phase 1, 3: 9개 / Phase 2: 20개 via point
하드웨어 Intel Core i7-9750 CPU @ 2.6 GHz

4.2 Scenario 1 -- Shallow Excavation (얕은 굴착)

700 Fig. 6: 두 시나리오의 global planning 결과. (a) Shallow excavation, (b) Deep excavation. Ground(주황), Target(흰색), Global planner result(녹색), Traversed trajectory(빨강).

700 Fig. 7: Scenario 1 - Shallow excavation 결과. (a) 전역 궤적 추종, (b) 정규화된 제어 입력, (c) 정규화된 파워 및 펌프 유량, (d) 정규화된 추정 외란. 빨간 점선은 물리적 한계를 나타냄.

주요 결과:

지표 값
Global trajectory 계산 시간 2.1초
Local planning 평균 계산 시간 21.7 ms (약 46 Hz)
물리적 제약조건 만족 모든 제약조건 충족
외란 추정 버킷 실린더에서 마찰 모델 명확히 관측, 붐/암에서는 토양 상호작용 외란이 더 큼

4.3 Scenario 2 -- Deep Excavation (깊은 굴착)

700 Fig. 8: Scenario 2 - Deep excavation 결과. (a) 전역 궤적 추종, (b) 정규화된 제어 입력, (c) 정규화된 파워 및 펌프 유량, (d) 정규화된 추정 외란.

주요 결과:

지표 값
Global trajectory 계산 시간 1.6초
Local planning 평균 계산 시간 30.2 ms (약 33 Hz)
물리적 제약조건 만족 모든 제약조건 충족
외란 크기 Scenario 1보다 큰 외란 (버킷이 토양에 더 깊이 침투)

4.4 두 시나리오 비교

항목 Shallow Excavation Deep Excavation
궤적 형태 넓은 영역 tip-grading 삼각형 체적 최대화
목표 도달 단일 pass 반복 필요
Global 계산 2.1 s 1.6 s
Local 계산 21.7 ms (46 Hz) 30.2 ms (33 Hz)
외란 크기 상대적 소 상대적 대
제약 만족 O O

5. 한계 및 향후 연구

논문이 명시한 가정·한계 (원문에 근거가 있는 것):

요약자 판단 (원문에 없는 추가 평가 — 참고용):


한마디로 정리


심화 탐구 기록

[!info] 탐구 일자: 2026-05-24 Claude와의 Q&A를 통해 논문의 핵심 수식과 기여도를 심층 분석한 기록. 이 섹션만 읽어도 논문의 수학적 구조와 독창성을 완전히 파악할 수 있도록 정리함.


Q1. 논문의 3가지 기여도 — 독창성과 차별성 분석

기여 1: Global + Local Planner 통합

기존 연구의 한계:

기존 접근법 고려한 것 빠진 것
Trajectory Optimization [6,7,9] 굴착량·버킷 자세 등 작업(operational) 제약 힘/토크, 펌프 유량 등 물리적(physical) 제약을 실시간으로 보장 못함
MPC [12,13] 물리적 제약을 이동 수평선으로 처리 작업 수준 목표(어디를 얼마나 파야 하는가) 상위 계획 부재
비최적화 기반 [1,14-16] DMP 등으로 궤적 생성 제약조건 자체를 명시적으로 넣을 수 없음

이 논문의 차별점: 두 계획기를 역할 분담시켜 하나의 파이프라인으로 결합.

[!tip] 비유 기존에는 "네비게이션(경로)"만 있거나, "차량 안전장치"만 따로 있었는데, 이 논문은 네비게이션이 안전장치와 실시간으로 대화하면서 경로를 조정하는 시스템을 만든 것.

기여 2: 축소 차원 Global Trajectory Optimization

기존 trajectory optimization [6,7,9]은 전체 궤적을 시간 도메인에서 밀도 높게 이산화하여 최적화 → 변수 폭발로 수십 초~수 분의 오프라인 계산 필요.

이 논문의 차별점: Bernstein 다항식 파라미터화 + Phase 분할 조합.

기존: 시간 스텝 500개 × 상태 8차원 = 변수 4,000개
이 논문: 제어점 9~20개 × 3차원(실린더) = 변수 27~60개

[!tip] 비유 기존에는 궤적의 모든 점을 하나하나 결정했다면(점묘화), 이 논문은 소수의 꼭짓점만 결정하면 부드러운 곡선이 자동으로 생기는 방식(벡터 그래픽)을 사용. 결과적으로 Global planning이 1.6~2.1초만에 완료.

기여 3: 외란 추정 기반 실시간 제약 궤적 생성

기존 연구 문제
Trajectory Optimization [6,7,9] 오프라인 궤적 → 외란(토양 저항, 마찰) 발생 시 궤적 무효화, 물리적 제약 위반
MPC [12] 실시간이지만 단일 입력·분리 동역학만 → 4개 액추에이터의 커플링 제약 표현 불가
MPC [13] 커플링 고려했으나 ~3.3 Hz → 실시간 부적합 (최소 10~30 Hz 필요)

이 논문의 차별점: 3가지 기술 결합으로 해결.

  1. 모멘텀 기반 외란 추정기 (Eq. 11) → 토양 저항·마찰 등 미지 외란을 실시간 추정
  2. Feedback Linearization (Eq. 13) → 비선형 동역학을 선형 시스템으로 변환, MPC 계산 대폭 가속
  3. 선형화된 MPC (Eq. 14) → 물리적 제약 4종 유지하면서 빠른 최적화

[!tip] 비유 - 기존 방법: 날씨 예보 없이 항로를 미리 정해놓고 비행 (외란 무시) - [13]: 날씨를 보면서 조정하지만, 3초에 한 번만 조정 (너무 느림) - 이 논문: 바람(외란)을 실시간 감지하면서, 초당 30~46회 항로 재조정, 모든 한계 준수

[13]의 3.3 Hz 벽을 30~46 Hz로 10배 이상 뛰어넘은 것이 핵심.

종합

기여 기존 대비 없었던 것 독창성의 핵심
① 통합 프레임워크 작업 + 물리 제약을 한 파이프라인에서 동시 처리 두 레벨 계획기의 역할 분담 + 실시간 연결
② 축소 차원 최적화 전역 궤적을 수 초 이내에 생성 Bernstein 파라미터화 + Phase 분할로 변수 ~100배 축소
③ Disturbance-aware MPC 외란 추정 + 4종 물리 제약 + 30 Hz 이상 실시간 외란 추정 + FL + 선형 MPC의 3중 결합

세 기여는 서로 맞물려 작동: ②가 빠르게 전역 궤적 생성 → ③이 외란에 강건하게 실시간 추종 → ①이 둘을 하나로 엮음. 이 조합 자체가 독창성.


Q2. 굴착 체적(Excavation Volume) 공식 유도

기하학적 직관

굴착 체적은 버킷이 지나간 경로 위쪽의 흙 면적. 2D 단면에서:

z ↑
  │    ╭─── z_surf(x): 현재 지표면
  │   ╱ ░░░░░░░░░░░░░░░░   ← 이 영역이 "파낸 흙"
  │  ╱ ░░░░░░░░░░░░░░░░░
  │ ╱░░░░░░░░░░░░░░░░░╱
  │╱___○___○___○___○╱    ← 버킷 팁 경로 (waypoints)
  │   (x₀,z₀)  ...  (x_{n₂},z_{n₂})
  └──────────────────────→ x

$$\text{굴착 면적} = \text{지표면 아래 면적} - \text{버킷 경로 아래 면적}$$

지표면 아래 면적 (해석적 적분)

지표면이 다항식이므로:

$$A_{\text{surf}} = \int_{x_{n_2}}^{x_0} z_{\text{surf}}(x)\,dx = \left[ a_0 x + \frac{a_1}{2}x^2 + \frac{a_2}{3}x^3 + \cdots \right]{x{n_2}}^{x_0}$$

적분 방향 $x_{n_2} \to x_0$: 굴착기가 먼 곳에서 가까운 곳으로 파들어오므로 $x_0 > x_{n_2}$, 적분값 양수.

버킷 경로 아래 면적 (사다리꼴 근사)

버킷 팁이 이산 waypoints $(x_0, z_0), \ldots, (x_{n_2}, z_{n_2})$를 지나므로, 등간격 사다리꼴 공식 적용:

각 구간의 면적:

$$A_k = \frac{z_k + z_{k+1}}{2} \cdot \Delta x, \quad \Delta x = \frac{x_0 - x_{n_2}}{n_2}$$

전체 합산 후 전개:

$$A_{\text{path}} = \frac{\Delta x}{2}\left[(z_0 + z_1) + (z_1 + z_2) + \cdots + (z_{n_2-1} + z_{n_2})\right]$$

$$= \frac{\Delta x}{2}\left[z_0 + 2z_1 + 2z_2 + \cdots + 2z_{n_2-1} + z_{n_2}\right]$$

$\Delta x$ 대입:

$$\boxed{A_{\text{path}} = \frac{z_0 + 2z_1 + \cdots + 2z_{n_2-1} + z_{n_2}}{2n_2} \cdot (x_0 - x_{n_2})}$$

Swept Volume 조립

$$\boxed{V_{\text{swept}} = \int_{x_{n_2}}^{x_0} z_{\text{surf}}(x)\,dx - \frac{z_0 + 2z_1 + \cdots + 2z_{n_2-1} + z_{n_2}}{2n_2} \cdot (x_0 - x_{n_2})}$$

버킷 용량 제한

아무리 넓은 면적을 쓸어도 버킷이 담을 수 있는 양에 한계가 있으므로:

$$\boxed{V = \min{V_{\text{swept}},\; V_{\text{cap}}(\theta(\mathbf{L}_{n_2}))}}$$

비용함수에서의 역할

$$J_{\text{Phase2}} = w_d \sum_{k=1}^{n_2} |\mathbf{L}k - \mathbf{L}{k-1}|_W^2 - w_v V$$

항 역할
$w_d \sum |\mathbf{L}k - \mathbf{L}{k-1}|^2$ 실린더 이동거리 최소화 (에너지 절약)
$-w_v V$ 굴착 체적 최대화 (부호 반전으로 minimize에 편입)

"최소한의 에너지로 최대한 많은 흙을 퍼라"는 트레이드오프를 $w_d, w_v$로 조절.


Q3. Phase 1 & 3 최적화 문제 수식 유도

Bernstein 다항식 파라미터화의 동기

궤적 $\mathbf{q}_L(t)$를 직접 시간 이산화하면 변수가 폭발. 대신 정규화된 시간 $s \in [0,1]$에서 소수의 제어점 $\boldsymbol{\beta}_k$로 전체 궤적 표현.

Bernstein 기저 함수의 핵심 성질

$$B_{k,n}(s) = \binom{n}{k} s^k (1-s)^{n-k}$$

성질 수식 의미
비음수성 $B_{k,n}(s) \geq 0$ 항상 양수
단위 분할 $\sum_{k=0}^n B_{k,n}(s) = 1$ 가중 평균 구조
끝점 보간 $p(0)=\beta_0,\; p(1)=\beta_n$ 시작·끝점 정확히 통과
볼록 껍질 $\min_k \beta_k \leq p(s) \leq \max_k \beta_k$ 곡선이 제어점 범위를 벗어나지 않음

궤적 파라미터화:

$$q_{L,i}(s) = \sum_{k=0}^{n} \beta_{k,i}\; B_{k,n}(s), \quad s = \frac{t}{T}$$

속도 유도

Bernstein 미분 → 차수 1 낮은 Bernstein:

$$\dot{\mathbf{q}}L(t) = \frac{n}{T} \sum{k=0}^{n-1} (\boldsymbol{\beta}{k+1} - \boldsymbol{\beta}_k)\; B{k,n-1}(s)$$

속도의 "제어점"은 $\frac{n(\boldsymbol{\beta}_{k+1} - \boldsymbol{\beta}_k)}{T}$.

가속도 유도

$$\ddot{\mathbf{q}}L(t) = \frac{n(n-1)}{T^2} \sum{k=0}^{n-2} (\boldsymbol{\beta}{k+2} - 2\boldsymbol{\beta}{k+1} + \boldsymbol{\beta}k)\; B{k,n-2}(s)$$

역동역학으로 제어 입력 결정

Phase 1, 3에서 외란 $\boldsymbol{\Delta}_L = 0$이므로:

$$\mathbf{u}(t) = M_L(\mathbf{q}_L)\;\ddot{\mathbf{q}}_L + \mathbf{h}_L(\mathbf{q}_L, \dot{\mathbf{q}}_L)$$

$\boldsymbol{\beta}_k$와 $T$를 정하면 → 궤적 결정 → 속도·가속도 계산 → 역동역학으로 $\mathbf{u}(t)$ 결정. 결정 변수는 $\boldsymbol{\beta}_k$와 $T$뿐.

비용함수

$$\min_{\boldsymbol{\beta}_k,\; T} \int_0^T \frac{1}{2} \mathbf{u}^\top W_u\, \mathbf{u}\; dt$$

$s = t/T$ 치환 시 $dt = T\,ds$: - $T$ 커지면 → $\ddot{\mathbf{q}}_L$ 작아져서 → $\mathbf{u}$ 작아짐 (느리게 = 힘 적게) - 하지만 적분 구간 길어짐 → 총 에너지 증가 - 이 트레이드오프가 최적 $T^*$ 결정

제약조건

(a) 위치 제약 — 볼록 껍질 성질로 제어점만 검사:

$$\mathbf{q}_L^l \leq \boldsymbol{\beta}_k \leq \mathbf{q}_L^u, \quad k = 0, \ldots, n$$

(b) 속도 제약 — 속도 제어점으로 변환:

$$\dot{\mathbf{q}}L^l \leq \frac{n(\boldsymbol{\beta}{k+1} - \boldsymbol{\beta}_k)}{T} \leq \dot{\mathbf{q}}_L^u, \quad k = 0, \ldots, n-1$$

(c) 경계 조건:

$$\boldsymbol{\beta}0 = \mathbf{q}{L,\text{start}}, \quad \boldsymbol{\beta}n = \mathbf{q}{L,\text{end}}$$

(d) Phase 3 추가 제약 (토양 흘림 방지):

$$\theta_0 \leq \theta(\boldsymbol{\beta}_k) \leq \pi$$

최종 최적화 문제

$$\begin{aligned} & \min_{\boldsymbol{\beta}1, \ldots, \boldsymbol{\beta}{n-1},\; T} && \int_0^T \frac{1}{2} \mathbf{u}(\boldsymbol{\beta}, T)^\top W_u\, \mathbf{u}(\boldsymbol{\beta}, T)\; dt \ & \text{s.t.} && \mathbf{q}L^l \leq \boldsymbol{\beta}_k \leq \mathbf{q}_L^u && \text{(위치 제약)} \ & && \dot{\mathbf{q}}_L^l \leq \frac{n(\boldsymbol{\beta}{k+1} - \boldsymbol{\beta}k)}{T} \leq \dot{\mathbf{q}}_L^u && \text{(속도 제약)} \ & && \boldsymbol{\beta}_0 = \mathbf{q}{L,\text{start}},\; \boldsymbol{\beta}n = \mathbf{q}{L,\text{end}} && \text{(경계 조건)} \ & && \theta_0 \leq \theta(\boldsymbol{\beta}_k) \leq \pi && \text{(Phase 3만)} \end{aligned}$$

변수 수: 제어점 9개(양 끝 고정 → 자유 7개) × 실린더 4개 + $T$ 1개 = 29개. 기존 수천 개 대비 압도적 축소.


Q4. Phase 2 제약조건 #3과 #4 심층 분석 — α 각도와 삼각형 충돌 박스

[!question] 핵심 질문 Phase 2 제약조건 중 #3(α > 0)과 #4(버킷 몸체 ≥ 팁 궤적)는 유사해 보이는데, 왜 둘 다 필요한가?

α (여유 각도, clearance angle)의 정확한 정의

500

α는 버킷 팁(tip)에서 측정되는 두 방향 사이의 각도:

요소 설명
$v_E = (\dot{E}_x, \dot{E}_z)$ 버킷 팁의 속도벡터 — 팁이 실제로 이동하는 방향
바닥면(cutting edge) 연장선 버킷 바닥면을 팁 너머로 연장한 가상의 선
α 이 두 방향 사이의 각도

부호 규약:

  α > 0 (정상)                        α < 0 (위반)
  ┌──────┐                            ┌──────┐
  │버킷  │  ← 등면이 뒤에             │버킷  │  ← 등면이 앞에
  │ body │                            │ body │
  └──┬───┘                            └───┬──┘
     │팁                                  │팁
     ├── → v_E                            ├── → v_E
     └─── ↘ 바닥면 연장선 (더 깊음)       └─── → 바닥면 연장선 (더 얕음)
        α↗ (양수)                            α↘ (음수)
                                         ↑ 바닥면이 토양을 밀어냄!

[!info] 인터랙티브 시각화 α 각도와 제약 #3/#4의 차이를 SVG 그래프로 상세하게 시각화한 HTML 파일: constraint_3_vs_4_explanation (브라우저에서 열어 확인)

제약 #3과 #4의 핵심 차이

제약 #3 (α > 0) 제약 #4 (몸체 ≥ 궤적)
검사 대상 버킷 자세(θ) vs 현재 이동 방향(v_E) 버킷 형상(삼각형) vs 누적 팁 경로
시간 범위 현재 순간 (instantaneous) 과거 궤적 전체 (historical)
방지하는 위험 등면/바닥면이 현재 지표면을 미는 것 몸체가 미굴착 토양 속에 들어가는 것
수학적 성격 각도 조건 (α의 부호) 기하학적 위치 조건 (꼭짓점 vs 곡선)

Case A: α > 0이지만 #4 위반 — 왜 #3만으로는 부족한가

급곡선 궤적에서 발생하는 상황:

  z ↑
    │  ○ wp0 (높은 곳)                    [버킷 삼각형]
    │   ╲                                  ╱╲
    │    ╲ (내려감)               꼭짓점A→ ╱  ╲
    │     ○ wp1    ──── 궤적 높이 ─── ─ ─╱─ ─ ─ ← x=A에서 궤적
    │      ╲              꼭짓점이 궤적보다 ↓ 아래!
    │       ╲               (미굴착 토양 속에 몸체 진입)
    │        ○ wp2
    │         ╲                  ← v_E (위쪽으로 이동 중)
    │          ○ wp3 ●←현재 팁      α > 0 ✓ (팁이 선행)
    │         ╱
    │        ○ wp4 (올라감)
    └───────────────────────→ x

Case B: #4 만족이지만 α ≤ 0 — 왜 #4만으로는 부족한가

완만한 궤적에서 발생하는 상황:

  z ↑
    │  ───────────── z_surf (지표면) ──────────
    │                                  ╱╲ ← 등면이 앞!
    │  ○───○───○───○───●팁          ╱  ╲
    │        (팁 궤적)    ╲        ╱ body╲
    │                      ╲      ╱──────╲
    │                       → v_E    ↑
    │               바닥면 연장선 → (더 얕음)
    │                          α < 0 ✗
    └───────────────────────────────────→ x

제약 #4의 수학적 정식화 — 삼각형 충돌 박스

논문에서는 버킷 형상을 삼각형(triangular collision box)으로 단순화하여 검사:

각 waypoint $k$에서 버킷을 3개 꼭짓점 ${P_1^k, P_2^k, P_3^k}$으로 근사하고, 과거 waypoint들의 팁 위치를 잇는 구간별 선형 경로를 $\gamma(x)$로 정의하면:

$$z_{P_j^k} \geq \gamma(x_{P_j^k}), \quad \forall\, j \in {1,2,3}, \quad \forall\, k \in {1, \ldots, n_2}$$

즉, 매 waypoint에서 삼각형 충돌 박스의 모든 꼭짓점의 $z$ 좌표가, 해당 $x$ 좌표에서의 과거 팁 궤적 높이 이상이어야 한다. 이 제약은 $\mathbf{L}_k$의 함수로 표현되는 비선형 부등식으로 ALGLIB 솔버에 포함된다.

5가지 제약조건의 물리적 의미 종합

# 제약 수식/조건 왜 필요한가?
1 위치 제약 $x \in [x_{\min}, x_{\max}]$, $z_{\text{target}} \leq z \leq z_{\text{surf}}$ 작업 영역 내에서, 목표와 지표면 사이에서만 굴착
2 $\theta$ 단조 증가 $\theta_k \leq \theta_{k+1}$ 에너지 낭비 방지 (왔다갔다 안 함)
3 $\alpha > 0$ 바닥면 연장선 각도 > $v_E$ 각도 바닥면이 현재 지표면을 밀지 않도록
4 몸체 ≥ 궤적 $z_{P_j^k} \geq \gamma(x_{P_j^k})$ 몸체가 미굴착 토양에 들어가지 않도록
5 $\theta < \pi$ 회전각 상한 토양이 손목 방향으로 흘러 장비 고장 방지

[!important] 결론 #3은 "지금 이 순간"의 자세, #4는 "과거 궤적 전체와의 관계"를 검사한다.

3만으로는 몸체가 과거 궤적을 관통하는 것을 막지 못하고,

4만으로는 바닥면이 지면을 미는 것을 막지 못한다.

둘 다 필요하다.


Q5. Local Planning MPC 물리적 제약조건 — 왜 4가지 모두 필요한가

[!question] 핵심 질문 유압 굴착기의 4가지 물리적 제약조건 (10a)~(10d)는 각각 무엇을 의미하며, 왜 4가지를 모두 동시에 적용해야 하는가?

유압 굴착기는 하나의 디젤 엔진 → 유압 펌프 시스템이 4개 액추에이터(스윙 모터 + 붐·암·버킷 실린더)를 동시에 구동한다. 이 "자원 공유" 구조가 제약조건의 핵심이다.

(10a) 힘/토크 제한 — 개별 액추에이터 한계

$$\mathbf{u}^l \leq \mathbf{u} \leq \mathbf{u}^u$$

각 액추에이터가 낼 수 있는 힘/토크의 하드웨어적 상·하한:

액추에이터 제어 입력 의미
스윙 모터 $T_U$ 캐빈 좌우 회전 토크 (Nm)
붐 실린더 $F_B$ 붐 상하 이동 유압력 (N)
암 실린더 $F_A$ 암 굽힘/펼침 유압력 (N)
버킷 실린더 $F_K$ 버킷 회전 유압력 (N)

하한 $\mathbf{u}^l$(음의 힘)도 존재하는 이유: 유압 실린더는 양방향(밀기/당기기) 모두 한계가 있기 때문.

(10b) 파워 제한 — 공유 에너지원 한계

$$p = \mathbf{u}^\top \dot{\mathbf{q}}_L \leq p^u$$

전개하면:

$$p = T_U \cdot \dot{\psi}_U + F_B \cdot \dot{L}_B + F_A \cdot \dot{L}_A + F_K \cdot \dot{L}_K$$

4개 액추에이터가 동시에 소비하는 총 기계적 파워. 모두 하나의 엔진 → 하나의 유압 펌프에서 공급받으므로, 개별 힘이 한계 내에 있더라도 총합이 펌프 최대 파워를 초과하면 안 된다.

[!example] 비유 가정집 전기: 에어컨(1,500W), 전자레인지(1,200W), 드라이어(1,000W) 각각은 개별 용량 이내. 하지만 동시에 켜면 총 3,700W → 차단기(3,000W) 트립! 굴착기도 마찬가지: 붐을 빠르게 올리면서 동시에 암을 빠르게 접으면 총 파워 초과 위험.

(10c) 실린더 변위 제한 — 물리적 이동 범위

$$\mathbf{L}^l \leq \mathbf{L} \leq \mathbf{L}^u$$

유압 실린더의 최소 수축 길이와 최대 신장 길이. 물리적 스토퍼(stopper)에 의한 하드웨어 한계.

  실린더:  ◄────────────────────────────►
           L^l (완전 수축)          L^u (완전 신장)
                 ├════════╗
                 │ 피스톤  ║═════════│
                 ├════════╝         │
                         ↑
                    현재 위치 L (이 범위 내)

이 제약이 없으면 최적화기가 실린더가 끝까지 가서 벽에 부딪히는(bottoming out) 궤적을 생성할 수 있다.

(10d) 펌프 유량 제한 — 가장 복잡한 제약

$$f_i = A_i(\text{sgn}(\dot{\mathbf{q}}_L))^\top \dot{\mathbf{q}}_L^{\text{abs}} \leq f_i^u, \quad i = 1, 2$$

기호 의미
$A_i$ 각 펌프가 담당하는 실린더들의 단면적 행렬
$\text{sgn}(\dot{\mathbf{q}}_L)$ 실린더 이동 방향 (신장/수축) → 단면적이 달라짐
$\dot{\mathbf{q}}_L^{\text{abs}}$ 실린더 속도의 절대값
$f_i^u$ 펌프 $i$의 최대 유량
$i = 1, 2$ Hyundai HX300L은 2개의 유압 펌프 사용

유량 = 단면적 × 피스톤 속도. 유압 실린더는 밀 때(신장)와 당길 때(수축)의 단면적이 다르므로(로드 측은 면적이 작음), $\text{sgn}(\dot{\mathbf{q}}_L)$로 방향을 판별하여 적절한 단면적을 선택한다.

  밀 때 (신장):                  당길 때 (수축):
  ┌─────────────┐              ┌─────────────┐
  │ ╔═════════╗ │  큰 면적      │ ╔═════╗     │  작은 면적
  │ ║         ║ │              │ ║     ║─── │  (로드가 차지)
  │ ╚═════════╝ │              │ ╚═════╝ ↑  │
  └─────────────┘              └─────────rod─┘

각 펌프의 최대 유량 $f_i^u$를 넘으면 유압 오일 공급 부족으로 실린더가 의도한 속도로 움직이지 못한다.

4가지 제약의 관계 — 개별 vs 시스템 제약

flowchart TB
    subgraph 개별["개별 액추에이터 제약"]
        A["(10a) 힘/토크 한계<br/>각 관절의 최대 힘"]
        C["(10c) 실린더 변위 한계<br/>각 관절의 이동 범위"]
    end

    subgraph 시스템["시스템 전체 (자원 공유) 제약"]
        B["(10b) 파워 한계<br/>엔진 총 에너지 공급"]
        D["(10d) 펌프 유량 한계<br/>유압 오일 공급량"]
    end

    A -.->|"개별 OK라도"| B
    A -.->|"개별 OK라도"| D
    B ---|"에너지 관점"| D

    style 개별 fill:#e8f4fd,stroke:#4a90d9
    style 시스템 fill:#fde8e8,stroke:#d94a4a
상황 (10a) (10b) (10c) (10d) 결과
붐만 천천히 올림 ✅ ✅ ✅ ✅ 정상
붐을 최대 힘으로 빠르게 올림 ✅ ⚠️ ✅ ⚠️ 파워/유량 초과 위험
4개 관절 동시 최대 속도 ✅ ❌ ✅ ❌ 엔진·펌프 과부하
실린더 끝까지 신장 후 더 신장 ✅ ✅ ❌ ✅ 물리적 충돌

[!important] 핵심 (10a)과 (10c)는 "각 관절 개별"의 한계, (10b)과 (10d)는 "공유 자원(엔진·펌프)"의 한계. 개별 제약만으로는 시스템 전체의 물리적 실현 가능성(feasibility)을 보장할 수 없으므로, 4가지를 모두 동시에 적용해야 한다.


Q6. 기존 연구 방법론 상세 분석 — 본 논문의 차별점 이해를 위한 배경

[!info] 관련 섹션: 1.2 기존 연구 분류 Q1이 "본 논문의 기여가 무엇인가"를 분석했다면, Q6은 "기존 방법이 구체적으로 어떻게 작동했는가"를 파고든다. 기존 방법의 내부 구조를 알아야 차별점이 명확해진다.

부류 1: Trajectory Optimization [6,7,9] — 오프라인 전체 궤적 최적화

작동 원리: 굴착 시작부터 끝까지의 전체 경로를 한 번에 오프라인으로 최적화. 비용함수(에너지 최소화 등)와 제약조건(힘 한계, 토크 한계 등)을 넣고 수학적 솔버로 "최적 경로"를 구한다.

[입력] 굴착기 동역학 모델 + 비용함수 + 제약조건
         ↓
   비선형 최적화 솔버 (offline, 수 분~수 시간)
         ↓
[출력] t=0 ~ t=T 까지의 전체 궤적 (관절각도, 속도, 힘)
한계 설명
Offline 계산 전체 궤적 사전 계산 → 실시간 재계획 불가
외란 취약 돌, 마찰 변화 등 실제 토양 조건에 대응 불가
제약조건 위반 가능 외란이 크면 사전 궤적 추종 시 힘 한계 초과 위험

부류 2: Receding Horizon / MPC [12,13] — 이동 수평선 최적화

MPC는 "미래를 조금씩 내다보면서 매 순간 최적 행동을 결정"하는 방법:

시점 t=0:  [현재]━━━━━━[예측 N스텝]  → 첫 제어 입력만 실행
시점 t=1:       [현재]━━━━━━[N스텝]  → 다시 최적화, 첫 입력만 실행
시점 t=2:            [현재]━━━━━━    → 반복...
[12] Bender et al. (2017) — Offset-free MPC
[13] Wind et al. (2020) — Nonlinear MPC
부류 3: 비최적화 기반 [1,14-16]
연구 방법 한계
[1] Son (DMP) 전문가 굴착 동작을 학습·재현 물리적 제약조건을 명시적으로 고려 불가
[14] Yang (Reachability Map) 도달 가능한 버킷 자세를 오프라인 계산 동적 제약조건 미반영
[15,16] Jelavic (계층적 제어) Walking excavator용 계층적 최적화 제약 우선순위 충돌 시 feasibility 보장 불가

최종 비교

비교 항목 [6,7,9] Traj. Opt. [12] Offset-free MPC [13] NMPC 본 논문
실시간 재계획 ❌ Offline ✅ ✅ (느림) ✅ 30Hz+
동역학 모델 Full 비선형 Decoupled 단순 Double Integrator Full → FL로 선형화
관절 간 커플링 ✅ ❌ ❌ ✅
외란 추정 ❌ △ (적분기) ❌ ✅ 모멘텀 기반
물리 제약 4종 ✅ △ (입력만) △ ✅ 전부
계산 속도 수 분 빠름 ~3.3Hz 33~46Hz

[!tip] 핵심 기존 오프라인 방법 [6,7,9]은 정확하지만 외란 대응 불가, 기존 MPC [12,13]는 실시간이지만 동역학이 너무 단순하거나 느렸다. 본 논문은 두 부류의 장점을 합쳐 "풀 동역학 + 모든 물리 제약 + 외란 보상 + 30Hz+"를 동시 달성했다.


Q7. 개별 기법의 독창성 평가 — "시스템 논문"의 기여 구조

[!info] 관련 섹션: 1.3 연구 기여 본 논문에서 사용한 Feedback Linearization, 모멘텀 기반 외란 추정, MPC+DDP는 각각 로봇공학에서 수십 년간 확립된 기법이다. 개별 기법의 독창성이 아니라, 통합 적용이 기여의 핵심.

Feedback Linearization (피드백 선형화)

모멘텀 기반 외란 추정

MPC + DDP

기여의 층위 구조

┌──────────────────────────────────────────────────────────────┐
│  Level 3: 시스템 통합 (본 논문의 주된 기여)                    │
│  "Global+Local 이중 구조로 모든 제약+외란+실시간을 동시 달성"    │
├──────────────────────────────────────────────────────────────┤
│  Level 2: 도메인 적용 (의미 있는 엔지니어링 기여)               │
│  • FL을 유압 굴착기의 closed-chain 동역학에 맞게 재정식화       │
│  • 모멘텀 외란 추정을 관절각↔실린더 변위 이중 표현에 적용        │
│  • 4가지 물리 제약(힘, 파워, 변위, 유량)을 MPC에 통합           │
├──────────────────────────────────────────────────────────────┤
│  Level 1: 개별 기법 (기존에 이미 확립됨)                       │
│  • Feedback Linearization (1980s~)                          │
│  • Momentum-based Disturbance Estimation (2003~)            │
│  • MPC + DDP + Augmented Lagrangian (1960s~2010s)           │
│  • Bernstein 다항식 파라미터화 (고전 수학)                      │
└──────────────────────────────────────────────────────────────┘

굴착기 도메인의 특수한 난점 — 단순 적용이 아닌 이유

굴착기 특유의 난점 일반 로봇팔과의 차이
Closed-chain kinematics 유압 실린더와 링크의 폐쇄 루프 → 야코비안 복잡
공유 파워원 4개 액추에이터가 하나의 유압 펌프를 공유 → 파워/유량 제약 커플링
극심한 외란 토양-버킷 상호작용은 수백~수천 N → 일반 로봇팔보다 훨씬 크고 예측 불가
대형 관성 30톤급 장비 → 제약조건 위반 시 안전 문제 심각

[!important] 결론 이런 유형의 논문은 제어/로봇 분야에서 "시스템 논문(systems paper)"이라 부르며, 알고리즘 독창성보다는 엔지니어링 통합의 완성도와 실용적 가치로 평가된다. 논문 스스로도 "To the best of our knowledge, this work presents the first real-time motion planning framework that satisfies constraints of a hydraulic excavator"라 표현하여, 기법이 아닌 프레임워크의 최초성을 주장한다.


Q8. Phase별 파라미터화 선택 근거 — Bernstein vs Waypoint, Bernstein vs B-spline

[!info] 관련 섹션: 3.3 Global Planning Q3이 Bernstein 파라미터화의 수식을 유도했다면, Q8은 "왜 이 선택을 했는가"라는 설계 근거를 분석한다.

Phase 2는 왜 Waypoint 이산화를 쓰는가?

Phase 2(굴착 구간)에는 위치에 종속된 복잡한 기하학적 제약조건이 있다:

이유 설명
불규칙한 지면 형상 지표면과 목표면 사이에서 자유로운 형태의 궤적이 필요. 매끄러운 다항식에 억지로 맞추면 제약 만족 어려움
위치별 제약조건 "각 지점에서" 지표면 아래·목표면 위·여유각 양수 등을 검사 → waypoint로 이산화하면 각 점에서 직접 검사 가능
Swept Volume 계산 사다리꼴 적분에서 waypoint가 직접 적분 노드로 활용됨
비매끄러운 궤적 허용 Deep excavation에서 삼각형 형태 궤적이 체적 최대화에 유리 → 매끄러운 다항식 곡선에 제한될 필요 없음

Phase 1 & 3는 왜 Bernstein 다항식을 쓰는가?

Phase 1(초기→지표면)과 Phase 3(굴착 완료→목표 위치)의 특성:

특성 Phase 1&3 Phase 2
제약조건 단순 box 제약 복잡한 기하학적 제약
궤적 형태 매끄럽고 에너지 효율적 각진 형태도 가능
이동 거리 긴 자유 이동 짧은 굴착 구간
최적화 목적 에너지 최소화 체적 최대화 + 안전

→ 소수의 제어점(9개)으로 매끄러운 궤적을 표현하는 Bernstein이 계산 효율과 궤적 품질 모두에서 유리.

Bernstein 대신 B-spline을 쓰지 않는 이유

논문 원문에서 명시한 Bernstein 선택 이유:

"Bernstein bases are used because the convex hull property allows to incorporate the box constraint for $\mathbf{q}_L$ and linear inequality constraints for $\dot{\mathbf{q}}_L$ easily."

비교 항목 Bernstein B-spline
Convex hull 범위 전역적 (global) — 모든 제어점의 볼록 껍질 국소적 (local) — 인접 제어점들의 볼록 껍질
끝점 보간 자동 ($p(0)=\beta_0$, $p(1)=\beta_n$) Clamped knot vector 설정 필요
미분 → 제약 변환 속도 제어점 = $\frac{n(\beta_{k+1}-\beta_k)}{T}$ → 선형 부등식으로 직접 변환 knot 의존적, 표현이 더 복잡
국소 제어 (local support) ❌ 없음 ✅ 있음 (하나의 제어점 변경이 일부분만 영향)
긴 궤적에서의 유연성 차수가 높아짐 낮은 차수 유지 가능

B-spline의 핵심 장점인 "국소 제어"가 이 문제에서 불필요한 이유: - 제어점이 9개뿐인 짧은 궤적 → 국소 수정 불필요 - 전체를 한 번에 최적화하므로 일부만 바꿀 필요 없음

Bernstein의 장점이 이 문제에 직접 대응하는 이유: - 전역 convex hull → 제어점만 검사하면 위치 제약 자동 보장 (Q3의 식 참조) - 자동 끝점 보간 → Phase 간 경계조건 설정이 간단 - 미분 = 제어점 차분 → 속도 제약이 선형 부등식으로 변환되어 솔버에 바로 입력 가능

[!tip] 한마디 정리 B-spline은 "긴 궤적의 일부분만 수정"할 때 강력하지만, 이 문제는 짧은 궤적을 한 번에 최적화하는 문제이므로 Bernstein의 끝점 보간 + 전역 convex hull + 미분 단순성이 더 유리하다.


Q9. 모멘텀 기반 외란 추정 — 수식 상세 유도

[!info] 관련 섹션: 3.4 Local Planning (MPC) — 외란 추정 (Eq. 11) 요약 본문의 Eq. 11이 어떻게 유도되고, 왜 저역통과 필터가 되는지를 Step별로 증명한다.

Step 1: 출발점 — 동역학에서 합성항을 분리

Euler-Lagrange 동역학에서 $\mathbf{h}\theta$를 코리올리스($C\theta$)와 중력($G_\theta$)으로 분리:

$$M_\theta\,\ddot{\mathbf{q}}\theta + C\theta\,\dot{\mathbf{q}}\theta + G\theta = J_\theta^\top \mathbf{u} + \boldsymbol{\Delta}$$

Step 2: 일반화된 모멘텀 정의 및 미분

$$\mathbf{p} = M_\theta\,\dot{\mathbf{q}}_\theta \quad (\text{일반화된 모멘텀})$$

곱의 미분법으로:

$$\dot{\mathbf{p}} = \dot{M}\theta\,\dot{\mathbf{q}}\theta + M_\theta\,\ddot{\mathbf{q}}_\theta$$

동역학에서 $M_\theta\,\ddot{\mathbf{q}}\theta = J\theta^\top \mathbf{u} + \boldsymbol{\Delta} - C_\theta\,\dot{\mathbf{q}}\theta - G\theta$를 대입:

$$\dot{\mathbf{p}} = (\dot{M}\theta - C\theta)\,\dot{\mathbf{q}}\theta + J\theta^\top \mathbf{u} - G_\theta + \boldsymbol{\Delta}$$

Step 3: 핵심 성질 $\dot{M}\theta = C\theta + C_\theta^\top$ 적용

$\dot{M}\theta - C\theta = C_\theta^\top$이므로:

$$\boxed{\dot{\mathbf{p}} = C_\theta^\top\,\dot{\mathbf{q}}\theta + J\theta^\top \mathbf{u} - G_\theta + \boldsymbol{\Delta}} \tag{A}$$

모멘텀의 변화율 = (알려진 항들) + (미지의 외란 $\boldsymbol{\Delta}$)

Step 4: 왜 $\boldsymbol{\Delta} = \dot{\mathbf{p}} - (\text{알려진 항들})$로 직접 구하지 않는가?

$\dot{\mathbf{p}}$를 구하려면 $M\ddot{\mathbf{q}}$가 필요 → 관절 가속도 $\ddot{\mathbf{q}}$ 필요 → 속도를 수치 미분해야 하는데, 미분은 센서 노이즈를 폭발적으로 증폭한다 (Q10에서 구체적 예시).

→ 대신 양변을 적분하여 가속도 계산을 회피:

$$\mathbf{p}(t) - \mathbf{p}(0) = \int_0^t (C_\theta^\top\,\dot{\mathbf{q}}\theta + J\theta^\top \mathbf{u} - G_\theta)\,d\tau + \int_0^t \boldsymbol{\Delta}\,d\tau$$

$\mathbf{p}(t) = M(\mathbf{q}(t))\dot{\mathbf{q}}(t)$이므로 관절 위치·속도만으로 계산 가능 → 미분 불필요, 적분만 사용.

Step 5: 관측기(Observer) 구조 — 피드백 추가

잔차 신호에 추정값 $\hat{\boldsymbol{\Delta}}$를 피드백으로 넣은 관측기 설계:

$$\hat{\boldsymbol{\Delta}} = K_E \underbrace{\left( \mathbf{p}(t) - \mathbf{p}(0) - \int_0^t \left(J_\theta^\top \mathbf{u} + C_\theta^\top\,\dot{\mathbf{q}}\theta - G\theta + \hat{\boldsymbol{\Delta}}\right) d\tau \right)}_{\displaystyle \mathbf{s}(t)} \tag{11}$$

핵심: 적분 안에 $\hat{\boldsymbol{\Delta}}$가 추가되었다. 이것이 단순 잔차와 다른 점.

Step 6: 저역통과 필터 특성 증명

$\mathbf{s}(t)$를 시간 미분:

$$\dot{\mathbf{s}} = \dot{\mathbf{p}} - (J_\theta^\top \mathbf{u} + C_\theta^\top\,\dot{\mathbf{q}}\theta - G\theta + \hat{\boldsymbol{\Delta}})$$

식 (A)에서 $\dot{\mathbf{p}}$를 대입하면 알려진 항이 모두 상쇄:

$$\dot{\mathbf{s}} = \boldsymbol{\Delta} - \hat{\boldsymbol{\Delta}}$$

$\hat{\boldsymbol{\Delta}} = K_E\,\mathbf{s}$이므로 양변을 미분하면:

$$\boxed{\dot{\hat{\boldsymbol{\Delta}}} = K_E(\boldsymbol{\Delta} - \hat{\boldsymbol{\Delta}})} \tag{LPF}$$

라플라스 변환: $\hat{\Delta}(s) = \frac{K_E}{s + K_E}\,\Delta(s)$ — 1차 저역통과 필터.

Step 7: 실린더 변위 기반 변환

MPC는 실린더 변위 기반 동역학을 사용하므로:

$$\hat{\boldsymbol{\Delta}}_L = J_L^\top\,\hat{\boldsymbol{\Delta}} = [\hat{\Delta}_U \;\; \hat{\Delta}_B \;\; \hat{\Delta}_A \;\; \hat{\Delta}_K]^\top$$

이 $\hat{\boldsymbol{\Delta}}_L$이 MPC 문제 (Eq. 14)의 동역학 모델에 입력되어, 외란을 인지한 미래 궤적 예측을 가능하게 한다.


Q10. 외란 추정 심화 Q&A — 수식의 세부 원리와 실용적 의미

[!info] 관련 섹션: 3.4 Local Planning — 외란 추정 Q9의 유도에서 생기는 자연스러운 후속 질문 7가지에 대한 상세 답변.

(1) $\dot{M}\theta = C\theta + C_\theta^\top$ 수식 유도

코리올리스 행렬의 표준 정의(Christoffel 기호):

$$C_{ij} = \sum_k c_{ijk}\,\dot{q}k, \quad c{ijk} = \frac{1}{2}\left(\frac{\partial M_{ij}}{\partial q_k} + \frac{\partial M_{ik}}{\partial q_j} - \frac{\partial M_{jk}}{\partial q_i}\right)$$

$(C + C^\top)_{ij}$의 $(i,j)$ 원소:

$$\sum_k (c_{ijk} + c_{jik})\,\dot{q}_k$$

$M$의 대칭성($M_{ij} = M_{ji}$)을 이용하여 $c_{ijk} + c_{jik}$를 전개하면, 6개 편미분 항 중 4개가 대칭적으로 상쇄되어:

$$c_{ijk} + c_{jik} = \frac{\partial M_{ij}}{\partial q_k}$$

따라서:

$$(C + C^\top){ij} = \sum_k \frac{\partial M{ij}}{\partial q_k}\,\dot{q}k = \dot{M}{ij} \quad \blacksquare$$

(2) 실제 모멘텀은 어떻게 측정하는가?

$$\mathbf{p}(t) = M_\theta(\mathbf{q}\theta(t))\,\dot{\mathbf{q}}\theta(t)$$

필요한 값 얻는 방법
$\mathbf{q}_\theta(t)$ (관절 각도) 엔코더로 직접 측정
$\dot{\mathbf{q}}_\theta(t)$ (관절 속도) 엔코더 미분 또는 속도 센서
$M_\theta(\mathbf{q}_\theta)$ (질량 행렬) CAD 데이터(링크 질량/관성) + 현재 관절 각도로 계산

"모델 예측 변화량"인 적분항 $\int(C_\theta^\top \dot{\mathbf{q}}\theta + J\theta^\top \mathbf{u} - G_\theta + \hat{\boldsymbol{\Delta}})\,d\tau$도 모두 $\mathbf{q}, \dot{\mathbf{q}}, \mathbf{u}$와 모델 파라미터로 계산 가능. 매 제어 주기(20ms)마다 수치 적분(누적 합산)으로 실장.

핵심: $\mathbf{p}(t)$는 미분 없이 현재 시점의 $\mathbf{q}, \dot{\mathbf{q}}$만으로 바로 계산 가능. 과거 값과의 차이 $\mathbf{p}(t) - \mathbf{p}(0)$도 단순 뺄셈.

(3) 왜 직접 $\boldsymbol{\Delta} = \dot{\mathbf{p}} - (\text{나머지})$로 구하지 않고 적분 경로를 택하는가?

직접 구하려면 $\dot{\mathbf{p}} = \dot{M}\dot{\mathbf{q}} + M\ddot{\mathbf{q}}$에서 관절 가속도 $\ddot{\mathbf{q}}$가 필요하다.

관절 위치 q(t)  ← 엔코더 측정 ✅
       ↓ 1회 미분
관절 속도 q̇(t)  ← 약간의 노이즈 ⚠️
       ↓ 1회 미분
관절 가속도 q̈(t) ← 노이즈 폭발! ❌  (아래 (4)번에서 구체적 예시)

적분 방식은 $\mathbf{p}(t) = M(\mathbf{q})\dot{\mathbf{q}}$만 필요하므로 $\ddot{\mathbf{q}}$ 계산을 완전히 회피한다.

적분은 노이즈를 평활화(smoothing)하고, 미분은 노이즈를 증폭(amplification)한다. 이것이 적분 형태를 택하는 근본적 이유.

(4) 미분에 의한 노이즈 증폭 — 구체적 숫자 예시

관절 속도를 1kHz(1ms 간격)로 샘플링, 센서 노이즈 ±0.01 rad/s:

시점(ms) 실제 $\dot{q}$ 측정 $\dot{q}$ 수치 미분 $\ddot{q}$ 실제 $\ddot{q}$
0 1.00 1.01 — 20
1 1.02 1.01 0 20
2 1.04 1.05 40 20
3 1.06 1.05 0 20
4 1.08 1.09 40 20

반면, 적분은 +/- 노이즈가 번갈아 나와 서로 상쇄되므로 결과가 안정적이다.

(5) 저역통과 필터의 직관적 이해

$\dot{\hat{\boldsymbol{\Delta}}} = K_E(\boldsymbol{\Delta} - \hat{\boldsymbol{\Delta}})$의 작동 원리:

추정값의 변화 속도  =  K_E  ×  (실제값 - 현재 추정값)
                      ↑          ↑
                    반응 속도    오차

주가의 이동평균선과 같은 원리: 매일의 등락(고주파)은 무시하고 전체 추세(저주파)만 추종.

$K_E$가 차단 주파수를 결정: - $K_E$ 크면 → 빠른 외란도 추정, 노이즈에 민감 (시정수 $\approx 1/K_E$ 짧음) - $K_E$ 작으면 → 매끄러운 추정, 느린 반응 (시정수 $\approx 1/K_E$ 김) - 굴착기의 토양 외란은 수백 ms~수 초 단위로 변하므로 적절한 $K_E$로 외란은 잡고 노이즈는 제거 가능

(6) $\dot{M}$은 알려진 항 — 왜 $C + C^\top$으로 바꾸는가?

$\dot{M}$도 계산 가능하다. $\dot{M}{ij} = \sum_k \frac{\partial M{ij}}{\partial q_k}\dot{q}_k$이므로, 편미분만 구하면 된다.

그러나 $C + C^\top$을 사용하는 것이 실용적으로 유리:

방법 추가 계산량
$\dot{M}$ 직접 계산 4-DOF 시스템: 편미분 40개 (별도 함수 필요)
$C + C^\top$ 사용 $C$는 동역학 계산에서 이미 구해진 행렬 → 전치+덧셈 1회

로봇 역학 라이브러리(RBDL, Pinocchio 등)가 $M, C, G$를 기본 제공하므로, $C$를 재활용하는 것이 실장에 편리하다. 수학적으로는 어느 쪽이든 동일한 결과를 얻으며, 이론적 필수가 아니라 실용적 선택이다.

(7) 이 기법은 본 논문 고유의 것인가?

아니다. 모멘텀 기반 외란 추정은 로봇공학에서 완전히 확립된 표준 기법이다.

연도 연구자 기여
2003 De Luca, Mattone 산업용 로봇팔에서 momentum observer 체계화
2006 De Luca et al. 충돌 감지 적용 → 협동 로봇 핵심 기술
2011 Haddadin et al. 안전한 인간-로봇 상호작용에 확장

상용 로봇(KUKA iiwa, Universal Robots, Franka Emika)에 이미 탑재되어 사람과의 충돌 감지에 사용 중.

본 논문에서 차별화된 부분은 기법 자체가 아니라 적용 방식:

항목 일반 로봇팔 (De Luca 등) 본 논문 (굴착기)
동역학 표현 관절 각도만 사용 관절 각도 + 실린더 변위 이중 표현
기구학 구조 Open chain Closed chain
외란 추정 후 용도 충돌 감지 → 정지/회피 MPC 동역학 모델에 주입 → 미래 궤적 예측에 반영

마지막 항목이 가장 의미 있는 차이: 일반 로봇은 "외력이 가해졌다"를 감지하여 반응하는 데 쓰지만, 본 논문은 추정된 외란을 예측·계획 단계에 직접 반영하여 제약조건 위반을 사전에 방지한다.


Q11. Fig. 6 — Global Planning 결과 해석: 얕은 굴착 vs 깊은 굴착 전략 차이

[!info] 관련 섹션: 4.2~4.3 Scenario 1 & 2 Fig. 6은 동일한 Global Planner가 목표 형상(target shape)에 따라 자동으로 다른 굴착 전략을 생성함을 보여주는 핵심 그림이다.

700

범례 (색상 의미)

색상 의미
주황선 (Ground) 현재 지표면 형상
흰선 (Target) 목표 굴착 형상 (여기까지 파야 함)
녹색선 (Global planner result) Global planner가 계산한 계획 궤적
빨간선 (Traversed trajectory) MPC(Local planner)가 실제로 추종한 궤적

(a) Shallow Excavation (얕은 굴착)

(b) Deep Excavation (깊은 굴착)

핵심 메시지

같은 알고리즘(비용함수 Eq. 4: 에너지 최소화 + 체적 최대화)이 목표 형상에 따라 자동으로 다른 전략을 생성한다:

목표 형상 자동 생성된 전략 물리적 이유
얕은 목표 넓고 얕게 긁기 (tip-grading) 얕은 깊이에서는 넓은 면적을 쓸어야 체적 최대화
깊은 목표 삼각형으로 깊이 파기 깊이 방향으로 침투해야 체적 최대화, 단 버킷 용량 $V_{\text{cap}}$ 제한 때문에 반복 필요

이것은 연구자가 두 전략을 별도로 설계한 것이 아니라, 최적화 문제의 해가 자연스럽게 분화된 결과이다.


Q12. Fig. 7 — Shallow Excavation 상세 결과: 4가지 성능 지표 분석

[!info] 관련 섹션: 4.2 Scenario 1 Fig. 7은 얕은 굴착 시나리오에서 Local Planner(MPC)의 4가지 성능 지표를 시간 축으로 보여준다. 빨간 점선은 모든 서브플롯에서 물리적 한계(제약조건)를 나타낸다.

700

(a) Global Reference Tracking — 전역 궤적 추종

4개 채널: Swing(캐빈 회전), Boom Cyl(붐 실린더), Arm Cyl(암 실린더), Bucket Cyl(버킷 실린더).

(b) Normalized Control Input — 정규화된 제어 입력

4개 액추에이터(Swing, Boom, Arm, Bucket)의 힘/토크 ($\mathbf{u}$)를 각 한계값으로 나누어 정규화.

(c) Normalized Power & Pump Flow Rate — 파워 및 펌프 유량

(d) Normalized Estimated Disturbance — 추정 외란

4개 축의 추정된 외란 $\hat{\boldsymbol{\Delta}}_L$을 시간에 따라 표시.

Fig. 7(d)의 ground truth는 어떻게 알 수 있는가?

[!warning] 핵심 포인트 이것은 시뮬레이션이기 때문에 가능한 것이다.

시뮬레이터가 매 시간 스텝마다 연구자가 설계한 외란 모델을 직접 계산하여 동역학에 주입한다:

외란 종류 사용 모델 물리적 의미
실린더 마찰 Modified LuGre friction 속도 반전 시 마찰력 변화 (stiction + sliding)
토양-버킷 상호작용 Modified FEE (Fundamental Earthmoving Equation) 버킷이 토양을 관통할 때의 저항력

시뮬레이터의 동역학 방정식에서:

$$\ddot{\mathbf{q}}\theta = M\theta^{-1}(-\mathbf{h}\theta + J\theta^\top \mathbf{u} + \underbrace{\boldsymbol{\Delta}}_{\text{시뮬레이터가 직접 계산}})$$

둘을 겹쳐 그려서 추정기의 정확도를 정량적으로 검증한 것이 Fig. 7(d)이다. 실제 굴착기에서는 이러한 비교가 불가능하며, 이것이 시뮬레이션 검증의 본질적 한계이다.


Q13. 실제 환경에서의 외란 Ground Truth 확보 방법

[!info] 관련 섹션: 5. 한계 및 향후 연구 Fig. 7(d)에서 시뮬레이션이기에 가능했던 ground truth 비교를 실제 환경에서는 어떻게 구현할 수 있는가? 이 질문은 논문의 "향후 연구: 실제 굴착기 검증"과 직결된다.

실제 환경에서 "완벽한 ground truth"는 존재하지 않으며, 근사적 방법들이 있다:

방법 1: 유압 압력 센서 기반 역산 (가장 현실적)

유압 실린더 양쪽 챔버의 압력을 측정하여 실제 힘을 역산:

$$F_{\text{cyl}} = P_{\text{head}} \cdot A_{\text{head}} - P_{\text{rod}} \cdot A_{\text{rod}}$$

이로부터 역동역학으로 외란을 계산:

$$\boldsymbol{\Delta} = M_\theta \ddot{\mathbf{q}}\theta + \mathbf{h}\theta - J_\theta^\top \mathbf{u}_{\text{measured}}$$

방법 2: 힘/토크 센서 직접 장착 (가장 정확)

버킷-암 연결부에 6축 force/torque 센서를 설치하여 토양 반력을 직접 측정.

방법 3: 통제된 실험 환경 (Instrumented Test Bed)

실내에 토양 빈(soil bin)을 구축하고, 바닥에 로드셀 배열 + 모션 캡처 시스템을 설치:

통제 요소 방법
토양 종류·밀도 표준화된 토양 사용, 다짐도 관리
토양 반력 측정 토양 빈 바닥에 로드셀 배열
버킷 위치 추적 모션 캡처 시스템 (Vicon 등)

방법 4: 이중 추정기 교차 검증 (센서 최소화)

서로 독립적인 원리의 추정기 2개(예: 모멘텀 기반 + 확장 칼만 필터)를 동시에 운용:

$$\text{신뢰도} \propto \text{두 추정기의 일치 정도}$$

방법 5: 데이터 기반 접근 (최근 연구 트렌드)

대량의 굴착 데이터를 수집하여 ML/DL 기반 외란 모델을 학습:

연구 단계별 권장 전략

단계 권장 방법 이유
초기 검증 ① 유압 압력 역산 + ④ 이중 추정기 최소 비용으로 추정기 신뢰도 확인
정밀 검증 ③ 토양 빈 + ② F/T 센서 통제된 환경에서 정확한 ground truth 확보
현장 적용 ① 압력 역산 + ⑤ 학습 모델 추가 센서 없이 실시간 외란 추정

[!important] 결론 실제 환경에서 외란의 완벽한 ground truth는 원리적으로 불가능하다. 유압 압력 센서 기반 역산(방법 1)이 가장 현실적인 "준 ground truth"이며, 대부분의 실험 논문에서 이 방법을 채택한다. 이 논문이 시뮬레이션에만 머문 이유도 실제 ground truth 확보의 어려움이 한 가지 원인일 것이다.


Q14. 시뮬레이션 환경 구성 상세 분석 — 논문이 말하지 않은 것들

[!info] 탐구 일자: 2026-05-24 요약 본문 4.1의 표를 넘어, 원문 Section V.A를 정밀 분석하여 각 구성 요소의 의미와 역할을 상세히 정리.

전체 구조: ROS + C++ 모듈화 아키텍처

시뮬레이션은 Fig. 2의 전체 흐름도를 그대로 소프트웨어로 구현한 것. 각 모듈이 ROS 노드로 개별 구현되어, 실제 로봇 시스템처럼 모듈 간 메시지 통신으로 동작한다.

┌─────────────────────────────────────────────────────────┐
│                    ROS 노드 구조                         │
│                                                         │
│  [환경 모델 노드]  →  [Global Planner 노드]              │
│                          │                              │
│                     x_r (전역 궤적)                      │
│                          ↓                              │
│  [외란 추정 노드]  →  [Local Planner (MPC) 노드]         │
│                          │                              │
│                    x_d, u_d (참조 궤적)                  │
│                          ↓                              │
│                   [Motion Controller 노드]               │
│                          │                              │
│                      u (제어 입력)                        │
│                          ↓                              │
│                   [PLANT (시뮬레이터) 노드]               │
│                       │      │                          │
│                  x (상태 피드백)                          │
└─────────────────────────────────────────────────────────┘

ROS를 사용하는 이유: 모듈 간 통신·시간 동기화·데이터 로깅을 표준화하므로, 시뮬레이션 코드를 실제 굴착기로 이식할 때 최소한의 수정만 필요. C++는 실시간 제약(30Hz+ MPC)을 만족시키기 위한 성능 확보 목적.

PLANT 시뮬레이터의 외란 모델 상세

논문 원문의 핵심 문장:

"external disturbance Δ is modeled with a steady-state modified LuGre friction model [26] for cylinder friction and modified Fundamental Earthmoving Equation (FEE) [7, 27] for soil-bucket interaction. Note that friction and soil-bucket interaction models are introduced only to construct a realistic simulation environment and are unknown to the planning algorithms."

핵심 포인트: 마찰/토양 모델은 시뮬레이터에만 존재하고, 계획 알고리즘은 이 모델을 전혀 모름. 이를 통해 외란 추정기의 성능을 공정하게 시험할 수 있다.

(1) Modified LuGre Friction — 실린더 마찰

마찰 성분 현상 일반 모델 LuGre
정지 마찰 (Stiction) 멈춘 상태에서 시작 시 큰 마찰 ✅ ✅
쿨롱 마찰 속도 무관 일정 마찰 ✅ ✅
점성 마찰 속도 비례 마찰 ✅ ✅
사전 활주 정지→이동 전환 시 미세 변형 ❌ ✅
Stribeck 효과 저속에서 마찰 감소 현상 ❌ ✅

"Modified"(정상상태 버전): 내부 상태 변수의 과도 응답 무시, 현재 속도에 대한 정상 상태 마찰력만 계산.

(2) Modified FEE — 토양-버킷 상호작용

FEE(Fundamental Earthmoving Equation)는 토양역학의 경험적 공식으로, 버킷 관통 시 저항력 예측:

FEE 구성 요소 의존 변수 크기
절삭 저항 버킷 날 길이, 관입 깊이, 토양 강도 수백~수천 N
날면 마찰 날면 각도, 토양 내부 마찰각 수백 N
토양 가속 버킷 속도, 토양 밀도 수십~수백 N
토양 자중 토양 밀도, 관입 깊이 수십~수백 N

솔버 구현 상세

모듈 솔버 세부 사항
Global Planner ALGLIB [29] 오픈소스 비선형 최적화. BLEIC(Bound/Linear Equality/Inequality Constrained) 알고리즘
Local Planner (MPC) DDP + Augmented Lagrangian [30] 제약조건을 벌칙 함수로 변환 → 비제약 DDP로 풀기. FL로 동역학이 선형화되어 DDP backward pass가 효율적 → 실시간 핵심

MPC 파라미터 의미: - 시간 수평선 1000ms: 현재로부터 1초 뒤 미래까지 예측 - 제어 이산화 20ms: 수평선 1000ms ÷ 20ms = 50스텝 예측 - Bernstein 제어점 9개(Phase 1,3) → 자유변수 29개, via point 20개(Phase 2)

Motion Controller

$$\mathbf{u} = J_\theta^{-\top}\left(\mathbf{h}\theta - \hat{\boldsymbol{\Delta}} + M\theta \mathbf{u}_{\text{PID}}\right)$$

MPC 출력의 미세 오차를 보정하는 하위 제어기. Feedback Linearization + PID 구조.

하드웨어 의미

Intel Core i7-9750 @ 2.6 GHz (2019년형 노트북급 CPU)에서 33~46Hz 달성 → 실제 굴착기 탑재 임베디드 컴퓨터에서도 충분히 구동 가능함을 시사.

결과 정규화 방식

정규화 대상 방법 이유
실린더 변위 $\mathbf{L}$ 상수 바이어스 + 상수 정규화 인자 절대값 범위가 커서 비교를 위해
제어 입력 $\mathbf{u}$ 상수 정규화 인자 각 액추에이터 한계 대비 %로 표시
파워, 유량 상수 정규화 인자 펌프 최대 용량 대비 %로 표시
외란 추정 제어 입력과 동일한 인자 외란 크기를 제어 용량 대비로 비교 가능

Q15. 시뮬레이터 유형 분석 — Vortex/AGX가 아닌 "순수 수식 기반 시뮬레이터"

[!info] 탐구 일자: 2026-05-24 이 논문의 시뮬레이터가 Vortex Studio나 AGX Dynamics 같은 상용 토양 시뮬레이터가 아니라, Isaac Sim 같은 MBD 모델에 해석적 힘 모델(LuGre, FEE)을 코드로 입력한 형태인지를 분석.

결론: 상용 시뮬레이터가 아닌, Isaac Sim보다도 단순한 "순수 수식 기반 시뮬레이터"

Euler-Lagrange ODE를 해석적으로 유도한 뒤 C++ 코드로 직접 구현한 것. 범용 물리 엔진(PhysX 등)도 사용하지 않았으며, 외란 $\boldsymbol{\Delta}$를 LuGre와 FEE의 닫힌 형태 수식(closed-form equation)으로 계산해서 더한 것이다.

세 가지 시뮬레이터 유형 비교

┌─────────────────────────────────────────────────────────────────┐
│  Level 3: Vortex Studio / AGX Dynamics                         │
│  • 토양을 이산 입자(DEM) 또는 연속체(FEM)로 모델링                │
│  • 버킷-토양 접촉을 물리 엔진이 자동 계산                        │
│  • 토양 변형, 유동, 붕괴 등 3D 현상 재현                         │
│  • 계산 비용: 매우 높음 (GPU 또는 대규모 병렬 처리)               │
├─────────────────────────────────────────────────────────────────┤
│  Level 2: Isaac Sim / MuJoCo / Gazebo + MBD                    │
│  • 범용 물리 엔진(PhysX, MuJoCo 등)이 강체 동역학 담당           │
│  • URDF/SDF로 로봇 모델 정의, 관절/접촉 자동 처리                │
│  • 토양은 별도 플러그인이나 외부 힘으로 추가                      │
│  • 계산 비용: 중간                                              │
├─────────────────────────────────────────────────────────────────┤
│  Level 1: ★ 이 논문의 시뮬레이터 ★                              │
│  • Euler-Lagrange ODE를 해석적으로 유도 → C++로 직접 구현         │
│  • M(q)q̈ + h(q,q̇) = J^T u + Δ  를 수치 적분                  │
│  • Δ = LuGre(q̇) + FEE(q, q̇, 지면형상)  해석적 수식             │
│  • 3D 물리 엔진 없음, 렌더링 없음                                │
│  • 계산 비용: 매우 낮음 (CPU 단일 코어)                           │
└─────────────────────────────────────────────────────────────────┘

Vortex/AGX와의 본질적 차이

비교 항목 이 논문 (FEE 기반) Vortex / AGX Dynamics
토양 표현 다항식 곡면 $z_{\text{surf}}(x)$ — 2D 단면 이산 입자(DEM) 또는 연속체 — 3D 볼륨
힘 계산 닫힌 형태 수식 1회 평가 수천~수만 입자 간 접촉력 반복 계산
토양 변형 버킷 경과 후 지면 형상을 수동 업데이트 입자/메시가 물리 법칙에 따라 자동 변형
흙 유동/붕괴 ❌ 모사 불가 ✅ 자연스럽게 발생
버킷-토양 접촉 기하학적 관입 깊이로 힘 계산 접촉 역학으로 자동 감지·계산
계산 비용 ~1ms 이하 수십~수백 ms (GPU 가속 시)

이런 단순한 시뮬레이터를 선택한 이유

  1. 외란의 "존재"가 중요하지, "정확도"가 아님 — 플래너는 어차피 외란 모델을 모름. "수백~수천 N의 미지 외란이 가해지는 상황"을 재현하는 데 FEE로 충분
  2. 계산 효율 — 알고리즘 개발 단계에서 수백~수천 번 반복 시뮬레이션 필요. Vortex/AGX로 한 번에 수 분 걸리면 개발 속도 저하
  3. 재현성과 제어 가능성 — 해석적 모델은 파라미터를 바꾸면 외란 크기를 정확히 제어 가능. DEM 시뮬레이션은 확률적 요소로 동일 조건 재현이 어려울 수 있음
  4. 논문의 한계로 명시 — 저자들도 "Future work may include validation in actual excavators"라고 인정

[!important] 시뮬레이션 환경의 설계 철학 "플래너는 모르지만, 시뮬레이터는 아는" 구조. 시뮬레이터는 LuGre + FEE로 현실적 외란을 생성하고, 플래너는 이를 전혀 모른 채 모멘텀 기반 추정만으로 대응해야 한다. 이 "정보 비대칭"이 외란 추정의 성능을 공정하게 검증하는 핵심이다.


Q16. 3D 렌더링의 실체 — RViz + CAD 메시 시각화

[!info] 탐구 일자: 2026-05-24 논문 Fig. 6에 보이는 3D 렌더링 화면(어두운 배경, 반투명 굴착기, 궤적 오버레이)은 Vortex/AGX 같은 물리 시뮬레이터의 렌더링이 아니라, 별도의 시각화 도구가 시뮬레이션 결과를 사후에 표시한 것이다.

700

물리 시뮬레이션과 시각화는 완전히 분리된 시스템

┌─────────────────────┐      ┌─────────────────────┐
│   물리 시뮬레이션     │      │    시각화 (렌더링)    │
│   (PLANT 노드)       │      │    (RViz)            │
│                     │      │                      │
│ • Euler-Lagrange    │ ROS  │ • 3D CAD 메시 표시    │
│ • LuGre 마찰        │ ───→ │ • 궤적 곡선 오버레이   │
│ • FEE 토양 저항      │ topic│ • 격자, 좌표축        │
│ • 수치 적분          │      │ • 스크린샷 → 논문 그림 │
│                     │      │                      │
│ 3D 렌더링 능력: ❌    │      │ 물리 계산 능력: ❌     │
└─────────────────────┘      └─────────────────────┘

시뮬레이터는 숫자만 계산하는 ODE 솔버이고, 눈에 보이는 3D 화면은 그 숫자를 별도의 시각화 도구가 그려준 것이다.

RViz를 사용한 것으로 판단하는 근거

  1. ROS 기반 시스템 — 모든 모듈이 ROS 노드로 구현됨. RViz는 ROS의 기본 시각화 도구
  2. 화면 특징 — 어두운 배경 + 격자 바닥 + 반투명 모델 + 마커 오버레이 = RViz의 전형적 렌더링 스타일
  3. CAD 메시 — ACKNOWLEDGMENT에 "Young Bum Kim (Hyundai Construction Equipment) for his kind support in the simulation setup" 기재 → Hyundai로부터 CAD 데이터와 모델 파라미터를 제공받았을 가능성 높음

Fig. 6 구성 요소 분석

화면 요소 RViz 구현 방식 데이터 출처
반투명 굴착기 (시작/끝 자세) RobotModel 플러그인 + URDF + CAD 메시 특정 configuration의 관절 각도
주황선 (Ground) Marker (LINE_STRIP) 지표면 다항식 $z_{\text{surf}}(x)$
흰선 (Target) Marker (LINE_STRIP) 목표 형상 $z_{\text{target}}(x)$
녹색선 (Global planner result) Marker (LINE_STRIP) Global planner 출력 버킷 팁 궤적
빨간선 (Traversed trajectory) Marker (LINE_STRIP) Local planner 실제 추종 궤적
격자 바닥 Grid 플러그인 RViz 기본 기능
어두운 배경 RViz 배경색 설정 궤적 가시성 향상 목적

시각화 데이터 흐름 (ROS 토픽)

# 개념적 코드 — PLANT 노드가 RViz로 데이터 전달
# 1. 관절 상태 → 로봇 모델 자세 업데이트
joint_state_pub.publish(JointState(
    name=["swing", "boom", "arm", "bucket"],
    position=[psi_U, theta_B, theta_A, theta_K]
))

# 2. 궤적 → 선 마커
trajectory_marker = Marker(type=LINE_STRIP)
trajectory_marker.color = ColorRGBA(r=0, g=1, b=0)  # 녹색
trajectory_marker.points = [Point(x=p[0], z=p[1]) for p in traj]
marker_pub.publish(trajectory_marker)

[!tip] 핵심 Fig. 6의 3D 렌더링은 물리 시뮬레이터가 직접 렌더링한 것이 아니라, 시뮬레이션 결과(관절 각도 시계열 + 궤적 좌표)를 RViz가 사후에 시각화한 것이다. Vortex/AGX처럼 "물리 엔진이 직접 렌더링하는 것"과는 본질적으로 다르다.


Q17. 구현 전략 — 2단계 프로토타입 + AGX Dynamics 이식 로드맵

[!info] 탐구 일자: 2026-05-24 본 논문의 알고리즘을 AGX Dynamics에 적용하기 전에, 논문과 동일한 가벼운 환경에서 먼저 알고리즘의 감을 잡는 2단계 프로토타입 전략을 수립. 상세 구현 가이드는 별도 노트 참조.

전략적 판단 근거

AGX에 바로 적용할 때의 리스크 프로토타입을 거치면
알고리즘 문제인지 시뮬레이터 설정 문제인지 구분 어려움 알고리즘이 이미 검증되어 있으므로, AGX에서 문제가 생기면 시뮬레이터 차이로 특정 가능
디버깅 사이클이 느림 (AGX 시뮬레이션 초 단위) 가벼운 환경에서 ms 단위 반복 실험 가능
MPC 가중치 튜닝에 시간 소모 프로토타입에서 미리 튜닝 완료, AGX에서 미세 조정만
기준 비교(baseline) 없음 프로토타입 결과가 baseline, AGX 토양 모델과의 차이 정량 비교 가능

로드맵 개요

flowchart LR
    P1["1단계: 물리 시뮬레이션<br/>Python + CasADi<br/>2~3주"] --> P2["2단계: RViz 시각화<br/>ROS 2 + URDF<br/>1~2주"]
    P2 --> P3["3단계: AGX 이식<br/>PLANT만 교체<br/>2~4주"]

    style P1 fill:#e8f4fd
    style P2 fill:#e8fde8
    style P3 fill:#fde8e8
단계 핵심 내용 산출물
1단계 Euler-Lagrange + LuGre + FEE를 Python으로 구현. Global/Local planner 코드화. 논문 Fig. 7/8 수준의 2D 플롯 재현 궤적 플롯, 제약조건 만족 확인
2단계 1단계 코드를 ROS 노드로 래핑. 단순 URDF 모델(직육면체)로 RViz 시각화. Fig. 6 스타일 3D 궤적 오버레이 실시간 3D 시뮬레이션 영상
3단계 PLANT(자체 ODE)를 AGX Dynamics로 교체. Global/Local Planner는 그대로 유지. AGX Terrain의 3D 토양 모델에서 MPC 성능 검증 AGX 환경 MPC 궤적 계획 동작

3단계에서 달라지는 것: PLANT만 AGX로 교체되고, Global/Local Planner와 외란 추정기는 그대로 유지. LuGre/FEE가 불필요해지고 AGX Terrain이 토양 반력을 자동 계산.

[!tip] 상세 구현 가이드 모듈 구성, 코드 구조, 기술 스택, 완료 기준 등 상세 내용은 별도 노트 참조: 굴착기 MPC 궤적계획 구현 전략 — 프로토타입에서 AGX Dynamics까지


Q18. Feedback Linearization 심층 분석 — 왜 비선형 MPC가 느리고, FL이 어떻게 10배 가속하는가

[!info] 탐구 일자: 2026-05-25 이전 세션에서의 Q&A를 바탕으로 Feedback Linearization의 계산적 원리, 이산화 기법, 제어 아키텍처에서의 역할을 심층 정리. 본문 3.4절의 수식을 넘어 "왜 빨라지는가"의 본질적 이유를 분석.


(1) 비선형 MPC가 느린 근본 원인

비선형 동역학 $\ddot{\mathbf{q}}_L = M_L^{-1}(-\mathbf{h}_L + \mathbf{u} + \hat{\boldsymbol{\Delta}}_L)$에서, DDP 솔버가 매 반복마다 수행하는 계산:

특히 유압 굴착기의 closed-chain kinematics에서 $M_L$의 구조가 복잡하여, 편미분 계산이 더욱 비싸다. 이것이 기존 연구 [13]이 ~3.3 Hz에 머문 근본 원인이다.

(2) FL이 가속하는 핵심 원리 — 상수 행렬 $A_c, B_c$

FL 적용 후 동역학:

$$\dot{\mathbf{x}} = A_c \mathbf{x} + B_c \mathbf{v}, \quad A_c = \begin{bmatrix} 0 & I \ 0 & 0 \end{bmatrix}, \; B_c = \begin{bmatrix} 0 \ I \end{bmatrix}$$

$A_c, B_c$가 상태 $\mathbf{x}$에 무관한 상수 행렬이므로:

비선형 MPC FL 기반 선형 MPC
$\frac{\partial f}{\partial \mathbf{x}}$: 매 스텝·반복마다 재계산 $A_d$: 한 번 계산, 영구 재사용
$\frac{\partial f}{\partial \mathbf{u}}$: 매 스텝·반복마다 재계산 $B_d$: 한 번 계산, 영구 재사용
$\frac{\partial^2 f}{\partial \mathbf{x}^2}$: 비영 텐서 $\frac{\partial^2 f}{\partial \mathbf{x}^2} = 0$ (헤시안 소멸)
DDP backward pass: $O(n^3)$ × 50스텝 × 반복 backward pass: 상수 행렬 곱만 → 극적 가속

[!important] 핵심 FL의 진짜 가치는 "비선형을 선형으로 바꿨다"는 수학적 변환 자체가 아니라, DDP의 매 반복에서 필요한 편미분·헤시안 계산을 사전 계산(pre-computation)으로 대체하여 계산량을 극적으로 줄인 것이다.

(3) 이산화에서 멱영(nilpotent) 성질 활용

$A_c$의 특수한 구조:

$$A_c^2 = \begin{bmatrix} 0 & I \ 0 & 0 \end{bmatrix}\begin{bmatrix} 0 & I \ 0 & 0 \end{bmatrix} = \begin{bmatrix} 0 & 0 \ 0 & 0 \end{bmatrix} = 0$$

행렬 지수의 급수 전개 $e^{A_c\Delta t} = I + A_c\Delta t + \frac{1}{2}(A_c\Delta t)^2 + \cdots$에서 2차 이상의 모든 항이 0:

$$e^{A_c\Delta t} = I + A_c\Delta t \quad (\text{정확한 이산화, 근사 없음})$$

따라서:

$$A_d = \begin{bmatrix} I & \Delta t \cdot I \ 0 & I \end{bmatrix}, \quad B_d = \begin{bmatrix} \frac{\Delta t^2}{2}I \ \Delta t \cdot I \end{bmatrix}$$

물리적 의미: $q_{k+1} = q_k + \Delta t\,\dot{q}_k + \frac{1}{2}\Delta t^2\,v_k$ — 이것은 등가속도 운동 공식과 정확히 동일하다. FL이 비선형 동역학을 "각 채널이 독립적으로 등가속도 운동하는 시스템"으로 변환한 것.

[!tip] 의미 일반적인 이산화(ZOH, 오일러 등)는 미소 $\Delta t$에서만 유효한 근사이지만, FL 후에는 $\Delta t$에 관계없이 정확한 이산화가 가능하다. 이것은 수치 안정성 면에서도 유리하다.

(4) 제어 아키텍처에서 FL의 이중 역할

FL은 시스템에서 두 곳에 등장한다:

① MPC 내부: 가상 입력 v_k를 실제 입력으로 변환
   u_d = FL(x_0, v_0*) = h_L + M_L·v_0* - Δ̂_L

② Motion Controller: MPC 출력의 미세 오차를 고주파 보정
   u = J_θ^{-T}(h_θ - Δ̂ + M_θ·u_PID)
MPC (Local Planner) Motion Controller
주파수 33~46 Hz ~1000 Hz
FL 역할 비선형 동역학을 선형화하여 최적화 가속 비선형 동역학을 상쇄하여 정밀 추종
출력 참조 궤적 $\mathbf{x}_d$ + 피드포워드 힘 $\mathbf{u}_d$ 실제 제어 입력 $\mathbf{u}$

MPC가 20~30ms마다 제약 만족 궤적을 계획하면, Motion Controller가 그 사이 1ms마다 FL+PID로 정밀하게 추종한다. MPC를 1000Hz로 돌릴 수 있다면 Motion Controller 없이 직접 제어 가능하지만, 현실적으로 MPC 계산이 20~30ms 걸리므로 그 사이를 PID가 메워주는 구조.

[!note] $\mathbf{x}_d \neq \mathbf{x}_r$ MPC 출력 $\mathbf{x}_d$는 별도 수식이 아니라 forward pass에서 자동으로 얻어지는 부산물이다. Global Planner의 $\mathbf{x}_r$은 물리적 제약을 미고려한 이상적 궤적이고, $\mathbf{x}_d$는 제약을 만족하도록 MPC가 조정한 현실적 궤적이다.

(5) FL + 외란 추정의 실제 동작 — 지하 돌에 부딪힐 때

외란 추정기가 큰 $\hat{\boldsymbol{\Delta}}$를 감지하면, MPC는 다음과 같이 반응한다:

  1. 외란 추정기 → 큰 $\hat{\boldsymbol{\Delta}}$ 감지
  2. FL 변환: $\mathbf{u} = \mathbf{h}_L + M_L\mathbf{v} - \hat{\boldsymbol{\Delta}}_L$에서 $\hat{\boldsymbol{\Delta}}_L$이 커짐
  3. MPC가 $\mathbf{x}_r$ 추종을 시도하지만, 힘 제약 $\mathbf{u}^u$ 때문에 불가
  4. $\mathbf{x}_d \neq \mathbf{x}_r$ 출력 → 매 제어 주기마다 편차 누적
  5. 결과적으로 원래와 다른 궤적 형성 (암묵적 우회)
상황 MPC 반응
작은 돌 감속 후 밀고 통과
중간 돌 힘 한계에 밀려 암묵적 우회 (궤적 이탈 누적)
거대 암반 진행 불가 (stuck) → Global Planner 재계획 필요 (본 논문 범위 밖)

[!warning] 중요한 구분 이것은 능동적 "회피"가 아니라, 제약조건에 의한 수동적 "밀림"이다. MPC는 돌의 존재를 모르며, 단지 "이 방향은 힘이 많이 든다"만 인지하여 갈 수 있는 곳으로 가는 것이다.

(6) FL의 설계 철학 — "알 수 있는 것"과 "알 수 없는 것"의 분리

$$\mathbf{u} = \underbrace{\mathbf{h}L + M_L\mathbf{v}}{\text{알 수 있는 것 (CAD 파라미터)}} - \underbrace{\hat{\boldsymbol{\Delta}}L}{\text{알 수 없는 것 (실시간 추정)}}$$

구분 파라미터 출처 처리 방법
알 수 있는 것 링크 길이, 질량, 관성 모멘트, 실린더 단면적 CAD 도면, 제조사 스펙 $M_L, \mathbf{h}_L$에 포함 → 모델 기반 상쇄
알 수 없는 것 실린더 마찰, 토양 반력, 유압 비선형성, 모델 오차 현장 조건 의존 $\hat{\boldsymbol{\Delta}}_L$로 실시간 추정

[!tip] 핵심 설계 철학 "알 수 있는 것(CAD 파라미터)은 모델에 넣고, 알 수 없는 것(마찰, 토양, 유압)은 외란으로 추정한다." FL의 $\mathbf{h}_L + M_L\mathbf{v}$ 항이 비선형 동역학의 "알려진 부분"을 정확히 상쇄하고, $\hat{\boldsymbol{\Delta}}_L$ 항이 "미지의 부분"을 실시간으로 보상한다. 이 분리 덕분에 복잡한 마찰 모델이나 토양 모델을 구축할 필요 없이, 센서 데이터만으로 동작한다.

(7) FL의 한계 — 완벽한 상쇄가 이루어지지 않을 때

FL은 $\hat{\boldsymbol{\Delta}}_L \approx \boldsymbol{\Delta}_L$를 가정하는데, 이 근사가 깨지는 경우:

상황 원인 영향
급격한 외란 변화 돌에 갑자기 부딪힘 저역통과 필터의 지연 → 일시적 $\hat{\boldsymbol{\Delta}}_L \neq \boldsymbol{\Delta}_L$
모델 오차 CAD 파라미터와 실제 값 차이 $M_L, \mathbf{h}_L$ 상쇄가 불완전 → 잔류 비선형성
센서 노이즈 엔코더 양자화 오차 모멘텀 계산 오차 → 외란 추정 정확도 저하

이때 비선형 동역학이 완전히 상쇄되지 않아 선형 모델과의 불일치(mismatch) 가 발생한다. 이를 MPC의 강건성(robustness) 이 흡수해야 하며, 이것이 MPC의 가중치($\tilde{Q}, \tilde{R}$) 튜닝이 중요한 이유 중 하나이다.

[!important] 결론 FL은 "마법의 변환"이 아니라, 모델이 정확할수록 잘 작동하는 모델 기반 기법이다. 본 논문이 모멘텀 기반 외란 추정기와 FL을 결합한 것은, 외란 추정이 모델 불확실성을 실시간으로 보상하여 FL의 상쇄 정확도를 높이기 위함이다. 두 기법은 상호 보강 관계에 있다.


Q19. 모멘텀 기반 외란 추정 — 직관적 설명

[!info] 탐구 일자: 2026-05-25 Q9~Q10의 수식 유도를 비유와 3단계로 단순화하여 직관적으로 재설명.


한 문장 요약

"내가 준 힘으로 예상되는 움직임"과 "실제 움직임"의 차이를 보면, 누군가 몰래 밀고 있는 힘을 알 수 있다.

비유: 눈 감고 쇼핑카트 밀기

당신은 눈을 감고 쇼핑카트를 밀고 있습니다.

┌─────────────────────────────────────────────┐
│                                             │
│   당신 →  🛒 ←← ??? (미지의 힘 = 외란)       │
│   (알고 있음: 내가 미는 힘, 카트 무게)         │
│   (모르는 것: 바닥 마찰, 누가 옆에서 미는지)    │
│                                             │
└─────────────────────────────────────────────┘

3단계로 이해하기

Step 1: "예상 모멘텀"과 "실제 모멘텀"

모멘텀 = 질량 × 속도 = $\mathbf{p} = M\dot{\mathbf{q}}$

이건 센서로 지금 바로 계산할 수 있다 (관절 위치·속도 측정 → 곱하기).

실제 모멘텀 = M(q) × q̇    ← 센서로 "지금" 바로 계산 ✅

한편, "외란이 없었다면 모멘텀이 이만큼이었을 것" 도 계산할 수 있다:

예상 모멘텀 = 초기값 + ∫(내가 준 힘 - 중력 - 마찰) dt    ← 적분으로 누적 ✅

Step 2: 차이 = 외란의 누적 효과

$$\underbrace{\mathbf{p}(t)}{\text{실제 모멘텀}} - \underbrace{\left(\mathbf{p}(0) + \int_0^t (\text{알려진 힘들})\,d\tau\right)}{\text{외란 없었으면 이만큼}} = \underbrace{\int_0^t \boldsymbol{\Delta}\,d\tau}_{\text{외란의 누적 효과}}$$

"실제로 이만큼 움직였는데, 내가 준 힘으로는 이만큼만 움직여야 했어. 차이가 나면 누군가 몰래 밀고 있는 거야."

Step 3: 피드백으로 "지금의 외란"을 추정

위의 차이(잔차)에 게인 $K_E$를 곱하면:

$$\hat{\boldsymbol{\Delta}} = K_E \times (\text{잔차})$$

이 구조는 자동으로 저역통과 필터가 된다:

$$\dot{\hat{\boldsymbol{\Delta}}} = K_E(\boldsymbol{\Delta} - \hat{\boldsymbol{\Delta}})$$

추정값의 변화 속도  =  K_E  ×  (실제 외란 - 현재 추정값)
                      ↑              ↑
                  반응 속도         오차

오차 크면 → 빠르게 따라감
오차 작으면 → 천천히 미세 조정  
노이즈로 +/- 진동하면 → 추정값은 중심에 머무름 (노이즈 제거)

왜 미분 대신 적분인가?

가장 단순한 방법은 $\boldsymbol{\Delta} = M\ddot{\mathbf{q}} + \mathbf{h} - \mathbf{u}$로 직접 구하는 것이지만, 가속도 $\ddot{\mathbf{q}}$가 필요하다.

  센서 측정(위치) → [미분] → 속도 (약간 노이즈 ⚠️)
                     → [미분] → 가속도 (노이즈 폭발! ❌)

  반면:

  알려진 힘들 → [적분] → 예상 모멘텀 (노이즈 상쇄, 매끄러움 ✅)

적분은 노이즈를 평활화하고, 미분은 노이즈를 증폭한다. 이것이 적분 경로를 택하는 이유.

최종 요약 박스

┌──────────────────────────────────────────────────┐
│                                                  │
│   외란 추정값 = K_E × (실제 모멘텀 - 예상 모멘텀)  │
│                                                  │
│   실제 모멘텀: M(q)·q̇           ← 센서로 바로 계산 │
│   예상 모멘텀: 초기값 + ∫(알려진 힘)dt ← 적분 누적  │
│                                                  │
│   차이가 나면? → 미지의 힘(외란)이 작용하고 있는 것! │
│                                                  │
└──────────────────────────────────────────────────┘

Q20. 가속도계 직접 측정 vs 적분 관측기 — 관절 가속도를 직접 잴 수 있다면?

[!info] 탐구 일자: 2026-05-25 "각 관절에 가속도계를 붙이면 적분 없이 외란을 바로 구할 수 있지 않나?" → 이론적으로 가능하나 실제로는 적분 방식이 우월한 4가지 이유.


이론적으로는 가능

가속도 $\ddot{\mathbf{q}}$를 직접 알면, 외란을 바로 계산할 수 있다:

$$\boldsymbol{\Delta} = M\ddot{\mathbf{q}} + \mathbf{h} - J_\theta^\top\mathbf{u}$$

미분도 적분도 필요 없이 대입 한 번이면 끝. 그런데 왜 로봇공학에서 이 방법을 잘 안 쓰는가?

문제 1: 가속도계가 측정하는 것 ≠ 관절 가속도

가속도계는 부착된 지점의 선형 가속도를 측정한다. 우리에게 필요한 것은 관절 각가속도 $\ddot{\mathbf{q}}_\theta$이다.

가속도계가 측정하는 것:              우리가 필요한 것:
a = [ax, ay, az] (m/s²)            q̈ = [ψ̈, θ̈_B, θ̈_A, θ̈_K] (rad/s²)
특정 지점의 선형 가속도              각 관절의 회전 가속도

멀티링크 시스템에서 한 지점의 가속도에는 모든 관절의 운동이 뒤섞여 있다:

$$\mathbf{a}{\text{sensor}} = \underbrace{\mathbf{a}{\text{관절 회전}}}{\ddot{q} \text{ 관련}} + \underbrace{\mathbf{a}{\text{구심}}}{\dot{q}^2 \text{ 관련}} + \underbrace{\mathbf{a}{\text{코리올리}}}{\dot{q}_i \dot{q}_j \text{ 관련}} + \underbrace{\mathbf{g}}{\text{중력}}$$

이것을 풀어서 $\ddot{\mathbf{q}}$를 역산하려면 운동학 모델 + 역변환이 필요하고, 여기서 모델 오차가 유입된다. "모델 없이 직접 측정"이라는 장점이 사라진다.

문제 2: 굴착기의 극한 진동 환경

진동 원인 주파수 크기
디젤 엔진 20~100 Hz 수 m/s²
유압 펌프 맥동 50~200 Hz 수 m/s²
버킷-토양 충격 광대역 임펄스 수십 m/s²
주행/선회 1~10 Hz 수 m/s²

가속도계는 이 모든 진동을 그대로 측정한다:

가속도계 출력:
                    ↓ 엔진 진동     ↓ 펌프 맥동
   ~~~~~~∧∨∧∨∧∨∧∨∧∨∧∨∧∨∧∨∧∨∧∨∧∨∧∨∧∨~~~~~
   ─────────────────────────────────────── ← 우리가 원하는 신호 (토양 외란)

모멘텀 관측기 출력:
   ───────╱──────╲──────────────────────── ← 저역통과 필터로 깨끗하게 추출

모멘텀 기반 방법은 구조적으로 저역통과 필터($K_E$) 가 내장되어 고주파 진동을 자동으로 걸러준다.

문제 3: "큰 수의 차이" 문제

$\boldsymbol{\Delta} = M\ddot{\mathbf{q}} + \mathbf{h} - J_\theta^\top\mathbf{u}$에서:

예시 (붐 실린더 기준, 개략적 크기):

  M·q̈  ≈  50,000 N   (관성력)
  h     ≈  45,000 N   (중력 + 코리올리)
  J^T·u ≈   8,000 N   (유압력)
  ──────────────────
  Δ     ≈  -3,000 N   (토양 외란)    ← 큰 수들의 작은 차이!

50,000과 45,000의 차이에서 3,000을 정확히 뽑아내려면, 각 항이 1% 미만의 오차여야 한다. 가속도 측정 오차가 여기에 더해지면 외란 추정값이 크게 왜곡된다.

반면, 모멘텀 기반 적분 방식은 이런 "큰 수의 차이"를 시간에 걸쳐 누적·평활화하므로 순간적 오차에 훨씬 덜 민감하다.

문제 4: 엔코더가 이미 가속도계보다 우수

비교 엔코더 (위치/속도) MEMS 가속도계
분해능 수만~수십만 카운트/회전 12~16비트
노이즈 매우 낮음 높음 (진동 환경에서 특히)
드리프트 없음 (절대 위치) 있음 (바이어스 드리프트)
가격 이미 장착됨 추가 비용
굴착기 환경 충격·먼지에 강인 진동에 민감

모멘텀 $\mathbf{p} = M(\mathbf{q})\dot{\mathbf{q}}$는 이미 장착된 고정밀 엔코더만으로 계산된다. 가속도계를 추가하면 비용은 올라가는데 정확도는 오히려 떨어질 수 있다.

결론

가속도계 직접 측정 모멘텀 기반 적분 관측기
이론적 단순함 ✅ 대입 한 번 △ 적분 + 피드백 구조
노이즈 강건성 ❌ 진동 직격 ✅ 저역통과 내장
모델 오차 민감도 ❌ "큰 수의 차이" ✅ 시간 평활화
필요한 센서 가속도계 (추가 장착) 엔코더 (이미 있음)
굴착기 적합성 ❌ 극한 진동 환경 ✅ 검증된 방식

[!important] 결론 가속도를 직접 측정할 수 있다면 원리적으로는 적분이 불필요하다. 하지만 실제 굴착기 환경에서는 가속도계의 노이즈·진동·모델 오차 문제로 인해, 이미 장착된 엔코더 + 적분 기반 관측기가 훨씬 실용적이다. 이것이 협동 로봇(KUKA, UR, Franka)부터 30톤 굴착기까지 모멘텀 기반 방법이 표준으로 자리 잡은 이유.


Q21. Feedback Linearization — 직관적 설명

[!info] 탐구 일자: 2026-05-25 Q18의 심층 분석을 비유와 최소 수식으로 재설명. FL이 무엇을 하는지, 왜 빨라지는지, 대가는 무엇인지를 직관적으로 정리.


한 문장 요약

"복잡한 기계의 특성을 미리 계산해서 상쇄시키면, 겉으로는 단순한 기계처럼 움직이게 만들 수 있다."

비유: 무게가 다른 4개의 문

당신은 4개의 문을 동시에 원하는 속도로 열어야 한다.

문 1: 10 kg (가벼움)     → 살짝 밀면 됨
문 2: 200 kg (무거움)    → 세게 밀어야 함
문 3: 50 kg + 스프링     → 밀다 보면 저항이 변함
문 4: 80 kg + 경사       → 중력이 도와주기도, 방해하기도 함

문제: 각 문의 무게·스프링·경사가 다르니까, "문 1을 1m/s로, 문 2를 0.5m/s로..." 같은 계획을 세우려면 매번 4개 문의 특성을 다 고려해야 한다. 계획 짜기가 매우 복잡하다.

FL의 아이디어: 각 문에 보조 장치를 달아서, 무게·스프링·경사를 미리 상쇄한다.

보조 장치 적용 후:

문 1: "가속도 = 내가 준 명령"    ← 단순!
문 2: "가속도 = 내가 준 명령"    ← 단순!
문 3: "가속도 = 내가 준 명령"    ← 단순!
문 4: "가속도 = 내가 준 명령"    ← 단순!

이제 4개의 문이 전부 "밀면 밀리는 단순한 물체" 처럼 동작한다. 계획 짜기가 극적으로 쉬워진다.

수식으로 보면

원래 (비선형) 동역학:

$$\ddot{\mathbf{q}} = M^{-1}(-\mathbf{h} + \mathbf{u} + \boldsymbol{\Delta})$$

입력 $\mathbf{u}$와 가속도 $\ddot{\mathbf{q}}$ 사이에 $M^{-1}$, $\mathbf{h}$, $\boldsymbol{\Delta}$가 복잡하게 얽혀 있다.

FL이 하는 일 — $\mathbf{u}$를 이렇게 설계한다:

$$\mathbf{u} = \underbrace{\mathbf{h}}{\text{비선형항 상쇄}} + \underbrace{M\mathbf{v}}{\text{원하는 가속도 스케일링}} - \underbrace{\hat{\boldsymbol{\Delta}}}_{\text{외란 보상}}$$

대입하면 $M^{-1}$, $\mathbf{h}$, $\boldsymbol{\Delta}$가 전부 사라지고:

$$\boxed{\ddot{\mathbf{q}} = \mathbf{v}}$$

"가속도 = 내가 준 명령". 이것이 FL의 전부이다.

┌──────────────────────────────────────────────────┐
│                                                  │
│   변환 전:  복잡한 비선형 시스템                     │
│            u → [M⁻¹, h, Δ 얽힘] → q̈              │
│                                                  │
│   변환 후:  가장 단순한 시스템 (이중 적분기)          │
│            v → q̈  (직통!)                         │
│                                                  │
└──────────────────────────────────────────────────┘

왜 이러면 MPC가 10배 빨라지는가

변환 전 (비선형 MPC) 변환 후 (선형 MPC)
동역학 $\ddot{q} = M^{-1}(-h + u + \Delta)$ $\ddot{q} = v$
MPC가 매 스텝 할 일 $M^{-1}$ 역행렬 + 편미분 + 헤시안 재계산 상수 행렬 곱 (사전 계산 완료)
비유 매번 4개 문의 무게·스프링을 다시 측정 "밀면 밀린다"만 기억하면 됨
결과 ~3.3 Hz (초당 3번) 33~46 Hz (초당 30번 이상)

핵심: $\ddot{q} = v$의 시스템 행렬 $A_c, B_c$는 상수이므로, MPC 솔버가 매번 다시 계산할 것이 없다.

FL의 대가 — $M$과 $\mathbf{h}$를 정확히 알아야 한다

FL은 $\mathbf{h}$와 $M$을 정확히 알아야 상쇄가 된다.

완벽한 상쇄:   u = h + Mv - Δ̂  →  q̈ = v  ✅  (이상적)

모델 오차 시:   u = h̃ + M̃v - Δ̂  →  q̈ = v + (잔류 오차)  ⚠️
                    ↑     ↑
                  부정확한 모델

그런데 $M$과 $\mathbf{h}$는 정확히 계산할 수 있을까? → 그렇다. 이 두 행렬의 모든 원소는 CAD 상수 × cos/sin(엔코더 측정값)의 조합으로, 미지 변수가 하나도 없다.

[!abstract] 상세 수식 전개 $M$과 $\mathbf{h}$의 원소별 전개, 각 변수의 출처(측정/CAD/미지) 색깔 구분은: 굴착기 동역학 M과 h 수식 해부 참조.

알 수 있는 것 (CAD) 알 수 없는 것 (현장)
링크 길이·질량·관성 → $M, \mathbf{h}$에 반영 마찰·토양·유압 비선형성 → $\hat{\boldsymbol{\Delta}}$로 추정

[!tip] FL의 설계 철학 "CAD로 아는 것은 모델로 상쇄하고, 모르는 것은 외란 추정기가 실시간으로 잡아준다." 두 기법이 서로의 약점을 보완하는 구조이다.

최종 요약

┌──────────────────────────────────────────────────┐
│                                                  │
│  FL = "비선형 특성을 미리 상쇄하는 보조 장치"        │
│                                                  │
│  • h (중력·코리올리)  → 모델로 상쇄                 │
│  • M (질량·관성)     → 모델로 스케일링              │
│  • Δ (외란)          → 추정기로 보상                │
│                                                  │
│  결과: q̈ = v  ("밀면 밀리는" 단순 시스템)           │
│  효과: MPC 계산 10배 이상 가속 (3Hz → 30Hz+)       │
│  대가: 모델이 부정확하면 상쇄가 불완전               │
│                                                  │
└──────────────────────────────────────────────────┘

Q22. Force/Torque 한계 $u^u, u^l$의 결정 — 실린더 스펙에서 어떻게 얻는가

[!info] 탐구 일자: 2026-05-26 Q5의 (10a) 힘/토크 제약 $\mathbf{u}^l \leq \mathbf{u} \leq \mathbf{u}^u$에서 상·하한을 실제로 어떻게 정하는가? 실린더 datasheet에 그대로 적혀 있는가, 아니면 추가 계산이 필요한가? APEX 구현 관점에서의 출처를 정리한다.


한 줄 결론

실린더 datasheet만으로는 force가 직접 나오지 않고 두 단계 계산이 필요하며, 실제 MPC 상한은 정격 압력 $P_{\max}$가 아닌 메인 릴리프 압력 $P_{\text{relief}}$ 로 묶어야 한다. 또한 extend/retract 방향의 면적 차이 때문에 $u^u$와 $u^l$은 일반적으로 절대값이 다르다.

Step 1: Datasheet에 직접 적힌 것

제조사 카탈로그에 보통 다음이 적혀 있다.

항목 기호 단위
보어 직경 (cylinder bore) $D$ mm
로드 직경 (rod) $d$ mm
스트로크 (stroke) $L$ mm
최대 사용 압력 (정격) $P_{\max}$ bar 또는 MPa
사용 유체, 작동 온도 등 — —

[!warning] 주의 굴착기 실린더 datasheet에 force 자체가 안 적혀 있는 경우가 많다. 동일 보어·스트로크라도 사용 압력에 따라 force가 바뀌기 때문. 일부 카탈로그에는 "Push force / Pull force at 350 bar" 식으로 특정 압력 기준 환산값을 표로 준다.

Step 2: Force 환산 — 면적 × 압력 (비대칭성 주의)

유압 실린더는 단동(extend) vs 복동(retract) 에서 단면적이 다르므로 force가 비대칭이다.

푸시(extend, 신장) — 보어측 가압:

$$F_{\text{push}}^{\max} = P \cdot A_{\text{bore}} = P \cdot \frac{\pi}{4} D^2$$

풀(retract, 수축) — 로드측 가압 (환형 면적):

$$F_{\text{pull}}^{\max} = P \cdot A_{\text{ann}} = P \cdot \frac{\pi}{4}\bigl(D^2 - d^2\bigr)$$

로드가 면적을 차지하므로 풀 방향 force가 항상 더 작다. 이 비대칭성 때문에 (10a)의 $u^l$과 $u^u$는 일반적으로 부호 비대칭이다.

Step 3: 수치 예시 (굴착기 boom 실린더 가정)

전형적인 30톤급 굴착기 boom 실린더:

파라미터 값
$D$ (bore) 140 mm
$d$ (rod) 95 mm
$P_{\max}$ 350 bar = 35 MPa

면적 계산:

$$A_{\text{bore}} = \tfrac{\pi}{4}(0.14)^2 = 0.01539\,\text{m}^2$$

$$A_{\text{ann}} = \tfrac{\pi}{4}(0.14^2 - 0.095^2) = 0.00831\,\text{m}^2$$

Force 환산:

$$F_{\text{push}}^{\max} = 35\,\text{MPa} \times 0.01539\,\text{m}^2 \approx 539\,\text{kN}$$

$$F_{\text{pull}}^{\max} = 35\,\text{MPa} \times 0.00831\,\text{m}^2 \approx 291\,\text{kN}$$

같은 실린더가 방향에 따라 약 1.85배 force 차이. (10a) 식에서 $u^u_{\text{boom}} \approx +539$ kN, $u^l_{\text{boom}} \approx -291$ kN.

Step 4: 실제 상한은 $P_{\max}$가 아닌 시스템 압력

실린더 정격압력은 "버틸 수 있는 압력"이지, "실제로 걸리는 압력"이 아니다. 실제 force 상한은 다음 중 가장 작은 값으로 묶인다.

한계 의미 일반적 크기
메인 릴리프 밸브 설정 압력 $P_{\text{relief}}$ 통상 $P_{\max}$의 70~85% 수준. 굴착기 시스템에서 보통 실효 상한 $P_{\max}$ × 0.7~0.85
포트 릴리프(과부하 밸브) 압력 충격하중 대응용. 메인보다 약간 높게 설정 $P_{\text{relief}}$ + 마진
펌프 토출 가능 최대압 펌프 사양 한계 모델별
구조 허용 force 핀, 부싱, 마운트 브래킷 한계 설계값
좌굴(Euler buckling) 한계 긴 스트로크 + 가는 로드 시 추가 제한 길이·로드 직경 의존
속도 의존 감쇠 빠르게 움직일 때 밸브/라인 손실로 실효 압력 감소 속도 함수

[!important] 핵심 $P_{\max}$ 대신 $P_{\text{relief}}$ 를 사용해야 보수적이고 실제와 정합한다. 굴착기 어떤 부하를 만나도 메인 릴리프가 열리는 압력 이상으로는 올라가지 않기 때문.

Doosan/HD현대인프라코어 굴착기의 경우 일반적인 메인 릴리프 설정은 30~35 MPa 범위이고, 실린더 보어/로드는 모델별 매뉴얼에 명시된다.

Step 5: 관절 토크 좌표계와의 연결

(10a)의 $\mathbf{u}$가 관절 토크 $[\tau_B, \tau_A, \tau_K]^T$ 표현이라면, 실린더 force를 Jacobian으로 변환해야 한다:

$$\boldsymbol{\tau} = J_{c \to \theta}^\top \cdot \mathbf{F}_{\text{cyl}}$$

여기서 $J_{c \to \theta}$는 실린더 변위 → 관절 각도 매핑의 Jacobian. 부호 규약상 extend가 $+$라면:

$$u^u = +F_{\text{push}}^{\max}, \quad u^l = -F_{\text{pull}}^{\max}$$

(절대값이 다른 비대칭 box constraint)

Step 6: APEX 구현 관점 — 실제로 얻는 4가지 경로

APEX 등가 MPC 설계 검토 의 Level B 구현 관점에서, $u^u, u^l$ 수치를 얻는 현실적 우선순위:

우선순위 방법 정확도 작업량
1 AGX 시뮬 모델 자체에 정의된 actuator force/effort 한계 노출 시뮬과 가장 정직 작음
2 굴착기 정비 매뉴얼의 메인 릴리프 설정 + 실린더 보어/로드 치수 → 직접 환산 실기와 정합 중간
3 실린더 datasheet의 $P_{\max}$ 사용 (보수적 과대평가) 안전 측 작음
4 로드셀/압력센서로 실측 (test bench 또는 굴착 시 압력 트레이스) 가장 정확 큼

[!tip] APEX 단계별 권장 - 현 단계 (smoke-test): 방법 1 — AGX가 노출하는 effort limit 그대로 사용 - Level B 진입: 방법 2 — 매뉴얼 기반 환산으로 정직한 수치 확보 - 실기 검증: 방법 4 — 압력 센서 트레이스로 검증

Step 7: 시각적 요약

실린더 datasheet           시스템 매뉴얼
┌──────────────┐          ┌──────────────────┐
│ D (보어)      │          │ P_relief         │
│ d (로드)      │  ──+──→  │ (메인 릴리프 설정)│
│ P_max (정격)  │          │                  │
└──────────────┘          └──────────────────┘
        │                          │
        └──── 면적 × 압력 환산 ────┘
                    │
                    ↓
        ┌─────────────────────────┐
        │ F_push^max (extend)      │ ← u^u
        │ F_pull^max (retract)     │ ← -|u^l|
        │ (비대칭, F_push > F_pull)│
        └─────────────────────────┘
                    │
              (관절 좌표면)
                    │
              J_{c→θ}^T 변환
                    │
                    ↓
        ┌─────────────────────────┐
        │ τ_B, τ_A, τ_K           │ ← (10a)의 u
        └─────────────────────────┘

최종 요약 박스

┌──────────────────────────────────────────────────┐
│                                                  │
│   u^u, u^l 결정 = 2단계 계산                      │
│                                                  │
│   1. Datasheet:  D, d, P_max                     │
│   2. Force 환산: F = (P_relief) × A              │
│                                                  │
│   • A_bore  > A_ann  → 비대칭 (|u^u| > |u^l|)    │
│   • P_relief < P_max (보통 70~85%)               │
│   • Datasheet에 "force" 직접 명기 안 됨           │
│                                                  │
│   APEX 구현: AGX actuator limit 우선,             │
│            매뉴얼 환산이 차선                      │
│                                                  │
└──────────────────────────────────────────────────┘

Q23. 외란 추정 — 시스템 관점에서 다시 보기

[!info] 탐구 일자: 2026-05-26 Q9~Q10이 수식 유도, Q19가 직관적 설명, Q20이 대안 센서와의 비교를 다뤘다면, Q23은 "외란이란 무엇이고, 왜 모멘텀 관측기가 천재적인가"를 시스템 설계 관점에서 재정리. 외란의 정체 분류, 블록 다이어그램, 회피한 함정, 5종 신호 의존성을 단일 표·도식으로 가시화.


(1) 외란 $\boldsymbol{\Delta}$의 정체 — 무엇이 들어있는가

식 (2)/(3)에서 $\boldsymbol{\Delta}$로 묶이는 모든 효과를 크기와 모델링 가능성별로 분류:

외란의 정체 일반적 크기 (붐 실린더 기준) 모델링 가능성
버킷-토양 반력 수백~수천 N 토양 종류·습도·밀도 의존, 정밀 모델 어려움
유압 시스템 비선형성 (밸브 dead zone, 압력 손실) 수십~수백 N 부분적 모델 가능, 동작점 의존
마찰 (실린더 packing, 핀 조인트) 수십~수백 N 정량화 어려움 (LuGre로 근사)
Mass property 오차 (URDF vs 실제) 수십 N 보상 가능하지만 시간 가변
외부 충격, 진동 광대역, 순간 수백 N 본질적으로 확률적

이 모든 것을 하나의 $\boldsymbol{\Delta}$로 묶어 추정하는 것이 모멘텀 관측기의 출발점. 각 성분을 따로 모델링하려는 시도(예: 시뮬레이터에서 LuGre + FEE 분리)는 검증용으로는 의미 있지만, 실제 굴착기에서는 모든 성분이 동시에 작용하므로 lumped 표현이 더 견고하다.

[!important] 외란 추정의 본질 "미지의 힘을 측정하는 가상 센서". 실제 토양 반력 센서가 없어도, 동역학 식의 잔차로부터 $\boldsymbol{\Delta}$를 역산한다. MPC가 $\hat{\boldsymbol{\Delta}}$를 알면 FL의 $\mathbf{u} = \mathbf{h}_L + M_L\mathbf{v} - \hat{\boldsymbol{\Delta}}_L$로 직접 상쇄 가능.


(2) 모멘텀 관측기 블록 다이어그램 — 텍스트 시각화

              ┌──────────────────────────────────────────┐
              │  MEASURED:  q_θ, q̇_θ                     │
              │  COMMANDED: u  (실린더 힘)                │
              │  MODEL:     M_θ(q), C_θ(q,q̇), G_θ(q), J_θ│
              └──────────────────┬───────────────────────┘
                                 ▼
              ┌──────────────────────────────────────────┐
              │  p(t) = M_θ(q_θ) · q̇_θ                   │
              │  (운동량 계산: 엔코더만으로 가능)            │
              └──────────────────┬───────────────────────┘
                                 │
                ┌────────────────┴────────────────┐
                ▼                                 ▼
       p(t) - p(0)                  ∫(J_θᵀu + C_θᵀq̇_θ - G_θ + Δ̂)dτ
       (실제 운동량 변화)               (모델 예측 + Δ̂ 자기 재귀)
                ▼                                 ▼
                     ─────────  잔차 r(t)  ─────────
                                  │
                                  ▼  × K_E
                            ┌──────────┐
                            │   Δ̂(t)   │  ← 출력
                            └────┬─────┘
                                 │
                       (다시 적분 안으로 피드백)
                                 │
                                 ▼
                     실린더 좌표 변환: Δ̂_L = J_Lᵀ Δ̂
                                 │
                                 ▼
                          [ MPC / FL 입력 ]

핵심 구조: - 상단 입력: 엔코더 신호(q, q̇) + 명령(u) + 모델(M, C, G, J) - 두 갈래 합산: 실제 모멘텀 변화 vs 모델 예측 + 자기 재귀 - 잔차 → K_E 곱: 게인이 LPF 시상수($1/K_E$)를 결정 - 자기 재귀: 적분 내부에 $\hat{\boldsymbol{\Delta}}$ 자신이 들어가는 것이 단순 잔차와의 결정적 차이 (LPF 동작의 비밀)


(3) 모멘텀 관측기가 회피한 4가지 함정

식 (11)의 진가는 무엇을 하지 않는가를 봤을 때 드러난다:

일반 외란 관측기 모멘텀 기반 관측기 (식 11)
$\ddot{\mathbf{q}}$ 측정/추정 필요 불필요 (운동량 정의 $\mathbf{p} = M\dot{\mathbf{q}}$로 흡수)
$\dot{M}$ 직접 계산 필요 불필요 ($\dot{M} = C + C^\top$ 항등식으로 자동 소거)
미분 연산으로 노이즈 증폭 적분만 사용 (인과적, 노이즈 평활화)
$\boldsymbol{\Delta}$ 형태 가정 필요 (LuGre, FEE 등) lumped 처리 — 형태 가정 없음

추가 보너스: - 수렴 보장: $K_E > 0$이면 무조건 $\hat{\boldsymbol{\Delta}} \to \boldsymbol{\Delta}$ (1차 LPF는 항상 안정) - 상수 외란: 정상상태에서 $\hat{\boldsymbol{\Delta}} = \boldsymbol{\Delta}$ (bias 없음) - 느린 외란: $|\dot{\boldsymbol{\Delta}}| \ll K_E |\boldsymbol{\Delta}|$ 영역에서 $\hat{\boldsymbol{\Delta}} \approx \boldsymbol{\Delta}$ (위상 지연만 존재) - 튜닝 파라미터 1개: $K_E$ (관절별 대각 행렬도 가능)만 조정


(4) 구현에 필요한 5종 신호/모델

식 (11)을 정직하게 구현하려면 다섯 가지가 모두 필요:

필요 양 출처 일반 굴착기 확보 가능성
$q_\theta, \dot{q}_\theta$ 관절 엔코더 ✅ 보통 확보
$M_\theta(q)$ rigid-body library + URDF/CAD ✅ CAD만 있으면 계산
$C_\theta(q, \dot{q})$, $G_\theta(q)$ rigid-body library ✅ 동일
$J_\theta$ (실린더 Jacobian) closed-chain 기하 모델링 ⚠️ 기하 모델링 필요
$u$ (실제 적용된 실린더 힘) 압력 센서 또는 force interface ⚠️ 속도 명령만 있으면 불가

마지막 줄이 핵심 병목. 정규화된 속도 명령만 있는 플랫폼(예: cmd ∈ [-1, 1])에서는 식 (11)을 그대로 적용할 수 없으며, 별도의 force interface(압력 센서 모델, 또는 시뮬레이터의 실린더 force 노출)를 구축하거나 외란 추정기 자체를 단순화해야 한다.

[!warning] 라벨링 주의 위 5종이 모두 갖춰지지 않은 상태에서 구현한 외란 추정은 "paper-inspired"로 정확히 라벨링해야 하며, "paper-equivalent"라 부르면 안 된다. 식 (11)의 LPF 보장(수렴, bias 없음 등)이 깨지기 때문이다. APEX 등가 MPC 설계 검토 에서 Level A vs Level B 구분이 정확히 이 지점을 반영한다.


(5) 한 장으로 보는 외란 추정의 흐름

[문제]    Δ를 알면 MPC가 정확해짐. 그런데 어떻게 측정?
          직접 계산 → q̈ 필요 → 노이즈 폭발

[발상 1]  운동량 p = Mq̇를 쓰면 미분 시 q̈가 자연스럽게 등장
[마법 1]  그 q̈를 동역학 식 (2)에 대입하면 Mq̈ 항이 소거됨
[마법 2]  Ṁ = C + C^T 항등식으로 Ṁ도 소거
          → ṗ = J^Tu + C^Tq̇ - G + Δ   (q̈, Ṁ 둘 다 사라짐)

[발상 2]  ṗ를 쓰는 대신 적분 형태로 변환
          ∫Δdτ = p - p(0) - ∫(J^Tu + C^Tq̇ - G)dτ
          (미분 연산이 완전히 사라짐)

[설계]    Δ̂ = K_E × [잔차],  잔차 안에 Δ̂ 자기 재귀
[결과]    Δ̂̇ = K_E(Δ - Δ̂)   ← 1차 LPF
          시상수 1/K_E로 진짜 Δ를 추적

[!tip] 한 줄 요약 "운동량을 쓰면 모든 미분이 적분으로 바뀌고, 결과는 진짜 외란의 LPF가 된다."


(6) Q9~Q21과의 관계 — 어디서 무엇을 보면 되나

주제 위치
Step별 수식 유도 ($\dot{p}$ 전개, 적분형 변환, LPF 증명) Q9
후속 7가지 Q&A ($\dot{M}=C+C^\top$, 노이즈 증폭, 측정 방법, $K_E$ 튜닝) Q10
비유 기반 직관 ("쇼핑카트", "예상 vs 실제 모멘텀") Q19
가속도계 직접 측정과의 비교 (왜 적분이 더 우월한가) Q20
시스템 관점 (외란 정체 분류, 블록도, 5종 의존성, 회피한 함정) Q23 (본 절)

[!important] 결론 Q23의 핵심 메시지는 두 가지. (1) 외란 추정은 "별도 센서 없이 미지의 힘을 보는 가상 센서"이다. (2) 그것을 가능케 하는 다섯 가지 신호/모델 중 하나라도 빠지면 식 (11)의 보장은 깨지므로, 단순화 구현은 반드시 "paper-inspired"로 라벨링해야 한다.