7.158 역동역학 기반 토크 최적화

1. 역동역학의 정의

역동역학(inverse dynamics)은 원하는 관절 궤적 $(\mathbf{q}(t), \dot{\mathbf{q}}(t), \ddot{\mathbf{q}}(t))$ 이 주어졌을 때, 이를 달성하는 데 필요한 관절 토크 $\boldsymbol{\tau}$ 를 계산하는 과정이다.

$\boldsymbol{\tau} = \mathbf{M}(\mathbf{q})\ddot{\mathbf{q}} + \mathbf{C}(\mathbf{q}, \dot{\mathbf{q}})\dot{\mathbf{q}} + \mathbf{g}(\mathbf{q})$

주어진 궤적에 대해 역동역학은 직접적으로 계산 가능하지만, 여유 자유도가 있거나 다중 과업을 수행해야 하는 경우에는 최적의 토크 분배가 요구된다.

2. 과업 공간 역동역학(Task-Space Inverse Dynamics, TSID)

2.1 문제의 구성

TSID는 과업 공간에서 원하는 가속도를 달성하면서 다양한 제약을 만족하는 관절 가속도와 토크를 최적화하는 방법이다. 결정 변수는 관절 가속도 $\ddot{\mathbf{q}}$ , 관절 토크 $\boldsymbol{\tau}$ , 접촉력 $\mathbf{f}_c$ 이다.

$\min_{\ddot{\mathbf{q}}, \boldsymbol{\tau}, \mathbf{f}_c} \quad \sum_k w_k \lVert \mathbf{J}_k\ddot{\mathbf{q}} + \dot{\mathbf{J}}_k\dot{\mathbf{q}} - \ddot{\mathbf{x}}_{d,k} \rVert^2 + w_\tau\lVert\boldsymbol{\tau}\rVert^2$

$\text{s.t.} \quad \mathbf{M}\ddot{\mathbf{q}} + \mathbf{h} = \mathbf{S}^T\boldsymbol{\tau} + \mathbf{J}_c^T\mathbf{f}_c$

$\boldsymbol{\tau}_{\min} \leq \boldsymbol{\tau} \leq \boldsymbol{\tau}_{\max}$

$\mathbf{f}_c \in \text{friction cone}$

이 문제는 이차 비용 함수와 선형 제약(동역학은 $\ddot{\mathbf{q}}$ , $\boldsymbol{\tau}$ , $\mathbf{f}_c$ 에 대해 선형)을 갖는 이차 계획(QP)이다.

2.2 동역학 일관성의 보장

동역학 등식 제약 $\mathbf{M}\ddot{\mathbf{q}} + \mathbf{h} = \mathbf{S}^T\boldsymbol{\tau} + \mathbf{J}_c^T\mathbf{f}_c$ 은 계산된 토크가 동역학적으로 일관됨을 보장한다. 이 제약이 없으면 과업 추종과 토크 최소화가 물리적으로 불가능한 해를 산출할 수 있다.

3. 계산된 토크 제어(Computed Torque Control)

3.1 피드포워드-피드백 구조

역동역학에 기반한 제어 법칙은 다음과 같은 피드포워드-피드백 구조를 갖는다.

$\boldsymbol{\tau} = \mathbf{M}(\mathbf{q})(\ddot{\mathbf{q}}_d + \mathbf{K}_d\dot{\mathbf{e}} + \mathbf{K}_p\mathbf{e}) + \mathbf{C}(\mathbf{q}, \dot{\mathbf{q}})\dot{\mathbf{q}} + \mathbf{g}(\mathbf{q})$

여기서 $\mathbf{e} = \mathbf{q}_d - \mathbf{q}$ 는 추종 오차이다. 역동역학에 의한 피드포워드 항이 비선형 동역학을 상쇄하고, PD 피드백 항이 추종 오차를 안정화한다.

3.2 토크 최적화의 역할

계산된 토크가 액추에이터의 한계를 초과하면, 최적화를 통해 제약을 만족하면서 과업 추종을 최대한 달성하는 토크를 계산해야 한다. TSID의 QP 정식화가 이를 체계적으로 처리한다.

4. 접촉력 분배 최적화

보행 로봇이나 다점 접촉 매니퓰레이터에서 접촉력의 분배는 고유하지 않다(여러 접촉점에 의한 과결정). 최적의 접촉력 분배는 다음의 기준을 최적화한다.

접촉력 노름 최소화: 접촉 표면에 가해지는 힘을 최소화하여 미끄러짐 위험을 줄인다.

$\min_{\mathbf{f}_c} \quad \lVert \mathbf{f}_c \rVert^2 \quad \text{s.t.} \quad \mathbf{J}_c^T\mathbf{f}_c = \boldsymbol{\tau}_{req}, \; \mathbf{f}_c \in \mathcal{K}$

여기서 $\mathcal{K}$ 는 마찰 원추의 곱이다.

안전 마진 최대화: 마찰 원추 경계로부터의 거리를 최대화하여 미끄러짐에 대한 강건성을 확보한다.

5. 토크 제약의 처리

5.1 관절 토크 포화

액추에이터의 최대 토크 $\tau_{i,\max}$ 를 초과하는 토크 명령은 실현 불가능하다. QP 정식화에서 구간 제약 $\boldsymbol{\tau}_{\min} \leq \boldsymbol{\tau} \leq \boldsymbol{\tau}_{\max}$ 으로 직접 부과되며, 토크가 포화되면 과업 추종 성능이 저하될 수 있다.

5.2 우선순위 기반 토크 할당

계층적 QP에 의해 상위 우선순위 과업(예: 균형 유지)에 토크를 먼저 할당하고, 잔여 토크 용량으로 하위 과업(예: 말단 장치 제어)을 달성한다. 토크 제약이 활성이면 하위 과업의 성능이 축소된다.

6. 실시간 구현

TSID QP의 규모는 통상 수십~수백 결정 변수, 수십~수백 제약으로, 현대 QP 해법기(qpOASES, OSQP)에 의해 1ms 이내에 풀 수 있다. 제어 주기(1~5ms)에서의 실시간 실행이 보장된다.

7. 참고 문헌

Sentis, L., & Khatib, O. (2005). “Synthesis of Whole-Body Behaviors through Hierarchical Control of Behavioral Primitives.” International Journal of Humanoid Robotics, 2(4), 505–518.
Del Prete, A., et al. (2016). “Implementing Torque Control with High-Ratio Gear Boxes and without Joint-Torque Sensors.” International Journal of Humanoid Robotics, 13(1).
Featherstone, R. (2008). Rigid Body Dynamics Algorithms. Springer.
Siciliano, B., et al. (2009). Robotics: Modelling, Planning and Control. Springer.

version: 1.0