2021 Real-time motion planning of a hydraulic excavator using trajectory optimization and model predictive control - 서울대_요약
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 문제 정의
- 유압 굴착기(hydraulic excavator)의 굴착 작업 자동화에는 다양한 operational constraints (굴착량, 버킷 자세 등)와 physical constraints (force/torque 한계, 펌프 유량 한계, 파워 한계 등)를 동시에 만족하는 궤적 계획이 필요
- 기존 trajectory optimization 방법은 전체 궤적을 한 번에 offline으로 최적화하므로, 운영 중 발생하는 외란(disturbance) -- 토양-버킷 상호작용, 유압 실린더 마찰 -- 에 취약
- 외란으로 인해 최적성 저하는 물론 제약조건 위반(constraint violation) 까지 발생 가능
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)
- Global + Local planner 통합: operational constraints와 physical constraints를 모두 고려
- 축소 차원 global trajectory optimization: 수 초 이내 계산 가능한 문제 정식화
- 실시간 제약 궤적 생성: 외란 추정 기반 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) |
실린더 변위 기반 동역학 (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) |
| 물리적 제약조건 만족 | 모든 제약조건 충족 |
| 외란 추정 | 버킷 실린더에서 마찰 모델 명확히 관측, 붐/암에서는 토양 상호작용 외란이 더 큼 |
- 목표 형상(target shape)을 관통하지 않도록 넓은 영역에서 궤적 생성
- Tip-grading-like motion 발생
- Global planning이 의도적으로 물리적 한계를 초과하더라도, local planning에서 제약조건 만족
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보다 큰 외란 (버킷이 토양에 더 깊이 침투) |
- 목표 형상이 깊으면 Phase 2에서 삼각형 형태의 궤적 생성하여 굴착 체적 최대화
- 반복 굴착을 통해 깊은 목표 형상에 점진적으로 도달하는 전략 (target-shape-aware excavation-volume-maximizing strategy)
- 더 큰 외란에도 불구하고 global reference 잘 추종 및 모든 물리적 제약조건 충족
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. 한계 및 향후 연구
- 시뮬레이션만 검증: 실제 굴착기에서의 실험 검증이 수행되지 않음
- 환경 사전 지식 가정: 지면 형상(ground shape)을 사전에 알고 있다고 가정 (LiDAR, 카메라 등으로 취득 가능하나 불확실성 미고려)
- 토양 모델의 단순화: Modified FEE 기반 토양-버킷 상호작용 모델은 시뮬레이션용이며, 실제 토양 조건의 다양성 미반영
- 단일 굴착 작업만 고려: 연속적인 반복 굴착, 트럭 적재 등 상위 수준 태스크 플래닝 미포함
- 향후 연구: 실제 굴착기에서의 알고리즘 검증 (논문 결론에서 명시)
한마디로 정리
- Global + Local planner 통합 프레임워크로 유압 굴착기의 실시간 궤적 계획 구현
- Force/torque, power, cylinder displacement, flow rate 등 4가지 물리적 제약조건 동시 충족 달성
- Feedback linearization으로 비선형 MPC를 선형화하여 30 Hz 이상 실시간 연산 달성
- 모멘텀 기반 외란 추정으로 마찰 및 토양 상호작용 disturbance-aware planning 구현
- Shallow/deep excavation 두 시나리오에서 제약 만족 + 전역 궤적 추종 검증 완료
- 실제 굴착기 실험 미수행 -- 시뮬레이션만 검증
심화 탐구 기록
[!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 등으로 궤적 생성 | 제약조건 자체를 명시적으로 넣을 수 없음 |
이 논문의 차별점: 두 계획기를 역할 분담시켜 하나의 파이프라인으로 결합.
- Global Planner → "어디를, 어떤 모양으로 파야 하는가?" (굴착 체적 최대화, 버킷 자세, 지면 형상 등 작업 제약)
- Local Planner (MPC) → "그 궤적을 따라가면서 기계가 부서지지 않게" (힘/토크, 파워, 실린더 변위, 유량 등 물리 제약)
[!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가지 기술 결합으로 해결.
- 모멘텀 기반 외란 추정기 (Eq. 11) → 토양 저항·마찰 등 미지 외란을 실시간 추정
- Feedback Linearization (Eq. 13) → 비선형 동역학을 선형 시스템으로 변환, MPC 계산 대폭 가속
- 선형화된 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}))}}$$
- $\theta$ 증가 → 버킷 닫힘 → $V_{\text{cap}}$ 증가
- $\theta \geq \pi$ → 토양이 반대편으로 흘러내림 → 장비 고장 위험 (Fig. 4)
비용함수에서의 역할
$$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. 기존 Trajectory Optimization 방법의 구체적 작동 원리 — Kim et al. (2013)
대표 논문: Y.B. Kim, J. Ha, H. Kang, P.Y. Kim, J. Park, F.C. Park, "Dynamically optimal trajectories for earthmoving excavators," Automation in Construction, vol. 35, pp. 568–578, 2013. (본 논문의 참고문헌 [7])
Trajectory Optimization의 5단계 레시피
| 단계 | 핵심 질문 | 기법 |
|---|---|---|
| ① 모델링 | 굴착기가 어떻게 움직이나? | Euler-Lagrange 동역학 |
| ② 파라미터화 | 궤적을 어떻게 표현하나? | B-스플라인 제어점 |
| ③ 비용 설계 | 무엇을 최소화하나? | 토크 제곱 적분 (에너지) |
| ④ 제약 설정 | 무엇을 지켜야 하나? | 관절/속도/토크/경로 한계 |
| ⑤ 최적화 | 어떻게 풀까? | NLP 솔버 (SQP 등) |
핵심 구조: 궤적 $\mathbf{q}(t)$를 B-스플라인 제어점 $c_{i,j}$로 파라미터화하여 무한 차원 문제를 유한 차원 NLP로 변환. 역동역학으로 토크 $\boldsymbol{\tau} = M\ddot{\mathbf{q}} + C\dot{\mathbf{q}} + G + \boldsymbol{\tau}_{\text{soil}}$를 계산하고, $\min \int_0^T \frac{1}{2}\boldsymbol{\tau}^\top W \boldsymbol{\tau}\,dt$를 SQP로 풀어 에너지 최소 궤적을 생성.
Kim (2013) vs Lee (2021, 본 논문) 비교
| 항목 | Kim (2013) | Lee (2021, 본 논문) |
|---|---|---|
| 계산 방식 | 오프라인 1회 | Global(수 초) + Local(실시간) |
| 외란 대응 | ❌ 불가 | ✅ 모멘텀 기반 외란 추정 |
| 제약 위반 | 외란 시 가능 | ✅ MPC가 매주기 보장 |
| 파라미터화 | B-스플라인 | Bernstein + waypoint |
Q5. 토양 반력 처리 — FEE 모델의 한계와 본 논문의 해결
Kim (2013)의 접근: FEE(Fundamental Equation of Earthmoving) 모델 사용
$$F_{\text{soil}} = \gamma g d^2 w N_\gamma + c \cdot d \cdot w \cdot N_c + c_a \cdot d \cdot w \cdot N_{ca} + q \cdot d \cdot w \cdot N_q$$
토양 파라미터($\gamma, c, c_a$ 등)를 사전에 설정하고 역동역학에 포함시켜 최적화. 한계: 파라미터 불확실성, 정적 모델, 균일 토양 가정 → 실제와 괴리.
본 논문(Lee 2021)의 해결: "예측 → 관측" 패러다임 전환
| Kim (2013) | Lee (2021) Global | Lee (2021) Local | |
|---|---|---|---|
| 철학 | 모델로 예측 | 아예 무시 | 관측으로 추정 |
| 토양 모델 | FEE (필수) | 불필요 | 불필요 |
| 비용 함수 | $\min \int \tau^2 dt$ (토양 반력 포함 토크) | $\min \sum |\Delta L|^2$ (실린더 거리, 토양 무관) | 참조 추종 + 입력 최소화 |
핵심 통찰: Phase 2의 비용 함수를 토크 최소화 → 실린더 이동 거리 최소화로 변경. 실린더 거리는 토양과 무관하게 기하학적으로 정확히 계산 가능.
Q6. Phase별 파라미터화 선택 이유 — Bernstein vs Waypoint vs B-spline
Phase 2에서 Waypoint를 쓴 이유
Phase 2는 시간 개념이 없는 기하학 문제 (비용 함수에 $t$ 미등장). 제약 조건이 비선형 경계(지표면·목표면 다항식)이므로 Bernstein의 convex hull 성질이 무용. Waypoint에서 직접 5가지 제약을 점별 검사하는 것이 자연스러움.
Phase 1&3에서 Bernstein을 쓴 이유
| Bernstein 장점 | 적용 |
|---|---|
| 전역 convex hull | $q_L^l \leq \beta_k \leq q_L^u$ → 전체 궤적 범위 보장 |
| 도함수 = 제어점 차분 | $\dot{q}L^l \leq n(\beta{k+1}-\beta_k)/T \leq \dot{q}_L^u$ → 속도 제약 자동 |
| 끝점 보간 | $p(0)=\beta_0$, $p(1)=\beta_n$ → 경계 조건 자동 |
B-spline을 쓰지 않은 이유
B-spline은 국소 convex hull(span별)만 제공 → 제약 처리 복잡. Local support(제어점이 근처만 영향)는 국소 수정에 유리하나, 전체를 한 번에 최적화하는 이 문제에서는 이점 없음. Bernstein이 제약 처리 + 구현 단순성 + 계산 속도 모두에서 우위.
Q7. 운동학 수식 유도 — 폐쇄 사슬 기구학
두 가지 Configuration 표현
$$\mathbf{q}_\theta = [\psi_U, \theta_B, \theta_A, \theta_K]^\top \quad \text{(관절 각도 — 동역학 유도에 자연스러움)}$$ $$\mathbf{q}_L = [\psi_U, L_B, L_A, L_K]^\top \quad \text{(실린더 변위 — 센서 측정 + 제약 선형화에 유리)}$$
폐쇄 사슬 기구학: 코사인 법칙에 의한 매핑
실린더 양 끝 피벗 위치와 코사인 법칙으로:
$$L_B^2 = a_B^2 + b_B^2 - 2a_B b_B \cos(\theta_B + \phi_B) \quad \Longleftrightarrow \quad \theta_B = \cos^{-1}!\left(\frac{a_B^2 + b_B^2 - L_B^2}{2a_B b_B}\right) - \phi_B$$
전체: $\mathbf{q}\theta = g_L(\mathbf{q}_L)$, $\mathbf{q}_L = g\theta(\mathbf{q}_\theta)$ — 전단사(bijective) 매핑.
야코비안: 속도 변환
$$\dot{\mathbf{q}}_\theta = J_L(\mathbf{q}_L)\dot{\mathbf{q}}_L, \quad J_L = \frac{\partial g_L}{\partial \mathbf{q}_L} \quad \text{(대각 행렬 — 각 실린더가 하나의 관절만 구동)}$$
물리적 의미: "실린더가 1mm 늘어나면 관절이 몇 rad 회전하는가."
버킷 팁 순방향 운동학
$$E_x = l_2\cos\theta_B + l_3\cos(\theta_B+\theta_A) + l_4\cos(\theta_B+\theta_A+\theta_K)$$ $$E_z = l_2\sin\theta_B + l_3\sin(\theta_B+\theta_A) + l_4\sin(\theta_B+\theta_A+\theta_K)$$ $$\theta = \theta_B + \theta_A + \theta_K$$
Q8. 동역학 수식 유도 — Euler-Lagrange → 좌표 변환
관절 각도 기반 동역학 유도
운동에너지 $T = \frac{1}{2}\dot{\mathbf{q}}\theta^\top M\theta \dot{\mathbf{q}}_\theta$에서 질량 행렬:
$$M_\theta(\mathbf{q}\theta) = \sum{i=1}^{4}\left[m_i J_{v,i}^\top J_{v,i} + I_i J_{\omega,i}^\top J_{\omega,i}\right]$$
Lagrange 방정식 적용 후 상태 공간 형태 (식 2):
$$\dot{\mathbf{x}}\theta = \begin{bmatrix}\dot{\mathbf{q}}\theta \ M_\theta^{-1}(-\mathbf{h}\theta + J\theta^\top \mathbf{u} + \boldsymbol{\Delta})\end{bmatrix}$$
$J_\theta^\top \mathbf{u}$: 가상 일의 원리에 의한 힘 변환 ($\tau_\theta^\top \delta q_\theta = \mathbf{u}^\top \delta q_L$ → $\tau_\theta = J_\theta^\top \mathbf{u}$).
실린더 변위 기반으로 좌표 변환 (식 3)
$\ddot{\mathbf{q}}_\theta = J_L \ddot{\mathbf{q}}_L + \dot{J}_L \dot{\mathbf{q}}_L$를 식 (2)에 대입하고 $J_L^\top$을 양변에 곱하면:
$$M_L \ddot{\mathbf{q}}_L = -\mathbf{h}_L + \mathbf{u} + \boldsymbol{\Delta}_L$$
핵심 관찰: $J_L^\top J_\theta^\top = (J_\theta J_L)^\top = I^\top = I$이므로, 실린더 동역학에서는 $\mathbf{u}$가 야코비안 변환 없이 직접 등장. 제어 입력이 원래 실린더 좌표에서 정의되었기 때문.
Q9. MPC 문제 정식화 및 피드백 선형화 — 완전 유도
4가지 물리적 제약의 물리적 의미
| 제약 | 수식 | 물리적 원인 |
|---|---|---|
| (10a) 힘/토크 | $\mathbf{u}^l \leq \mathbf{u} \leq \mathbf{u}^u$ | 실린더/모터의 최대 출력 |
| (10b) 파워 | $\mathbf{u}^\top\dot{\mathbf{q}}_L \leq p^u$ | 하나의 엔진이 4개 액추에이터에 파워 공급 |
| (10c) 변위 | $\mathbf{L}^l \leq \mathbf{L} \leq \mathbf{L}^u$ | 실린더 물리적 스트로크 한계 |
| (10d) 유량 | $A_i^\top \dot{\mathbf{q}}_L^{\text{abs}} \leq f_i^u$ | 2개 유압 펌프의 최대 유량 |
외란 추정기 유도
모멘텀 $\mathbf{p} = M_\theta\dot{\mathbf{q}}\theta$의 시간 변화율과 $\dot{M}\theta = C_\theta + C_\theta^\top$ 성질로부터:
$$\dot{\mathbf{p}} = C_\theta^\top\dot{\mathbf{q}}\theta - G\theta + J_\theta^\top\mathbf{u} + \boldsymbol{\Delta}$$
추정값 $\hat{\boldsymbol{\Delta}}$를 사용해 적분 관측기를 구성하면 (식 11), 미분하면:
$$\boxed{\dot{\hat{\boldsymbol{\Delta}}} = K_E(\boldsymbol{\Delta} - \hat{\boldsymbol{\Delta}})}$$
1차 저역통과 필터로 작동. 정상 상태에서 $\hat{\boldsymbol{\Delta}} \to \boldsymbol{\Delta}$ (외란이 충분히 느리게 변할 때).
비선형 MPC가 느린 이유
$\ddot{\mathbf{q}}L = M_L^{-1}(-\mathbf{h}_L + \mathbf{u} + \hat{\boldsymbol{\Delta}}_L)$에서, DDP 솔버가 매 반복마다: - $M_L^{-1}$ 계산: $O(n^3)$ - $\frac{\partial M_L^{-1}}{\partial q{L,j}} = -M_L^{-1}\frac{\partial M_L}{\partial q_{L,j}}M_L^{-1}$ 계산 (역행렬의 미분) - 이것을 4변수 × 50스텝 × 매 반복 → 수백 번의 역행렬 + 행렬 곱
피드백 선형화 유도
가상 입력 $\mathbf{v} \in \mathbb{R}^4$ 도입. $\ddot{\mathbf{q}}_L = \mathbf{v}$가 되도록 $\mathbf{u}$를 설계:
$$M_L\mathbf{v} = -\mathbf{h}_L + \mathbf{u} + \boldsymbol{\Delta}_L \quad \Longrightarrow \quad \boxed{\mathbf{u} = \mathbf{h}_L + M_L\mathbf{v} - \hat{\boldsymbol{\Delta}}_L}$$
대입하면 비선형항이 상쇄되고 ($\hat{\boldsymbol{\Delta}}_L \approx \boldsymbol{\Delta}_L$ 가정):
$$\ddot{\mathbf{q}}L = \mathbf{v} \quad \Longrightarrow \quad \dot{\mathbf{x}} = \underbrace{\begin{bmatrix}0 & I\0 & 0\end{bmatrix}}{A_c}\mathbf{x} + \underbrace{\begin{bmatrix}0\I\end{bmatrix}}_{B_c}\mathbf{v}$$
$A_c, B_c$가 상수 → 편미분 사전 계산 가능, 헤시안 = 0.
이산화 ($A_c^2 = 0$ 성질 활용)
$e^{A_c\Delta t} = I + A_c\Delta t$ (멱영 행렬)이므로:
$$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$ — 등가속도 운동 공식과 동일.
최종 선형화 MPC (식 14)
$$\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)$$ $$\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$$
동역학은 선형(상수 $A_d, B_d$), 제약은 비선형 유지 → DDP + Augmented Lagrangian으로 풀이. 비선형 MPC 대비 10배 이상 가속 (3.3 Hz → 33~46 Hz).
Q10. MPC의 제어 아키텍처 — Planner vs Controller
MPC는 "제어 입력까지 내는 Planner"
MPC 최적화의 결정 변수는 $\mathbf{v}_0^, \ldots, \mathbf{v}_{N-1}^$이고, 이로부터: - $\mathbf{u}_d = \text{FL}(\mathbf{x}_0, \mathbf{v}_0^)$ — 피드포워드 힘 (FL 변환) - $\mathbf{x}_d = A_d\mathbf{x}_0 + B_d\mathbf{v}_0^$ — 다음 참조 상태 (동역학 등식의 부산물)
$\mathbf{x}_d$는 별도의 수식이 아니라 forward pass에서 자동으로 얻어짐. $\mathbf{x}_d \neq \mathbf{x}_r$ — $\mathbf{x}_r$(Global)은 제약 미고려, $\mathbf{x}_d$(MPC)는 제약 만족 버전.
Motion Controller가 필요한 이유: 제어 주파수 문제
| MPC | Motion Controller | |
|---|---|---|
| 주파수 | 33~46 Hz | ~1000 Hz |
| 역할 | 제약 만족 궤적 + 힘 계획 | 정밀 추종 + 고주파 외란 거부 |
| 출력 | $(\mathbf{x}_d, \mathbf{u}_d)$ | 실제 $\mathbf{u}$ |
Motion Controller 수식 (원문): $\mathbf{u} = J_\theta^{-\top}(\mathbf{h}\theta - \hat{\boldsymbol{\Delta}} + M\theta \mathbf{u}_{\text{PID}})$
구조: 피드포워드($\mathbf{u}_d$) + 피드백 보정($K_p(\mathbf{x}_d - \mathbf{x})$). MPC가 20ms마다 계획을 주면, Motion Controller가 그 사이 1ms마다 정밀 추종.
[!tip] 핵심 MPC를 1000Hz로 돌릴 수 있다면 Motion Controller 없이 직접 제어 가능. 현실적으로 MPC 계산이 20~30ms 걸리므로, 그 사이를 PID가 메워주는 구조.
Q11. MPC의 암묵적 외란 회피 동작
지하 돌에 부딪힐 때의 MPC 반응
외란 추정기가 큰 $\hat{\boldsymbol{\Delta}}$를 감지 → MPC가 $\mathbf{x}_r$ 추종을 시도하지만 힘 제약 $\mathbf{u}^u$ 때문에 불가 → $\mathbf{x}_d \neq \mathbf{x}_r$ 출력 → 매 제어 주기마다 누적 → 결과적으로 원래와 다른 궤적 형성.
중요한 구분: "회피"가 아니라 "밀리는 것"
- MPC는 돌의 존재를 모름 — 단지 "이 방향은 힘이 많이 든다"만 앎
- 수동적 반응: 힘 한계에 밀려서 갈 수 있는 곳으로 감 (제약 반응)
- 능동적 회피 아님: 돌의 형상을 인식하고 경로를 재설계하는 것이 아님
| 상황 | MPC 반응 |
|---|---|
| 작은 돌 | 감속 후 밀고 통과 |
| 중간 돌 | 힘 한계에 밀려 암묵적 우회 (궤적 이탈 누적) |
| 거대 암반 | 진행 불가 (stuck) → Global Planner 재계획 필요 (본 논문 범위 밖) |
Q12. 데이터 기반 방법(DMP+PID)과의 비교
비교 대상: Son et al., "Expert-emulating excavation trajectory planning," IROS 2020 (본 논문 참고문헌 [1], 볼트 내 요약)
핵심 차이
| 비교 항목 | 데이터 기반 (DMP+PID) | 최적화 기반 (MPC, 본 논문) |
|---|---|---|
| 철학 | 전문가 모사 | 물리 기반 최적화 |
| 사전 필요 | 전문가 데이터 18세트 | CAD 동역학 파라미터 |
| 제약 보장 | ❌ 명시적 보장 없음 | ✅ 4종 물리 제약 명시적 보장 |
| 외란 대응 | 1차원 변조 (얕은 방향) | 다차원 최적 조정 |
| 범용성 | △ 학습 범위 내 보간 | ✅ 어떤 지형이든 최적화 |
| 계산 비용 | 매우 가벼움 (~1ms) | 무거움 (20~30ms) |
| 실기 검증 | ✅ 38톤 실기 (Doosan DX380LC) | ❌ 시뮬레이션만 |
| 모델 의존 | 낮음 | 높음 (CAD 파라미터) |
상호 보완 가능성
두 방법은 대립이 아니라 자연스러운 역할 분담으로 결합 가능: - DMP → Global Planner 대체: 전문가의 지혜가 담긴 자연스러운 초기 궤적 생성 - MPC → Local Planner 유지: DMP 궤적을 물리적 제약 내에서 안전하게 추종
→ "전문가처럼 자연스러우면서 물리적으로도 안전한 궤적" 동시 달성.
모델 의존성에 대한 재평가
MPC에 필요한 "정확한 모델"은 대부분 CAD에서 바로 구할 수 있는 파라미터:
| 파라미터 | 출처 | 구하기 쉬운가? |
|---|---|---|
| 링크 길이, 피벗 위치 | CAD 도면 | 🟢 |
| 링크 질량, 관성 모멘트 | CAD 3D 모델 | 🟢 |
| 실린더 단면적, 변위 한계 | 제조사 스펙 | 🟢 |
| 펌프 유량/파워 한계 | 제조사 스펙 | 🟢 |
| 실린더 마찰 | 온도/마모 의존 | 🔴 → $\boldsymbol{\Delta}$로 처리 |
| 토양 반력 | 현장 조건 의존 | 🔴 → $\boldsymbol{\Delta}$로 처리 |
| 유압 비선형성 | 밸브 특성 의존 | 🔴 → $\boldsymbol{\Delta}$로 처리 |
원문의 $\boldsymbol{\Delta}$ 정의: "lumped disturbance describing unmodeled dynamics, e.g. friction in hydraulic cylinders, soil-bucket interaction, and model uncertainties"
[!tip] 핵심 설계 철학 "알 수 있는 것(CAD 파라미터)은 모델에 넣고, 알 수 없는 것(마찰, 토양, 유압)은 외란으로 추정한다." 피드백 선형화 $\mathbf{u} = \mathbf{h}_L + M_L\mathbf{v} - \hat{\boldsymbol{\Delta}}_L$에서 $\mathbf{h}_L, M_L$은 CAD로 정확히 구한 값으로 비선형 동역학을 상쇄하고, $\hat{\boldsymbol{\Delta}}_L$은 실시간 추정으로 미지 요소를 보상. 결과적으로 복잡한 마찰/토양 모델 구축 없이 센서 데이터만으로 동작.
나의 의견
(작성 예정)