7.113 로봇 전신 운동 제어에서의 QP 응용

1. 전신 운동 제어의 개요

전신 운동 제어(Whole-Body Control, WBC)는 다자유도 로봇(휴머노이드, 사족 보행 로봇, 이동 매니퓰레이터 등)의 전체 관절을 통합적으로 제어하여 다수의 과업을 동시에 수행하는 방법론이다. 전신 운동 제어의 핵심은 다중 과업 목표와 물리적 제약을 이차 계획 문제(QP)로 정식화하여, 매 제어 주기에서 최적의 관절 토크 또는 가속도를 실시간으로 계산하는 것이다.

2. 동역학 모델

부유 기저(floating-base) 로봇의 운동 방정식은 다음과 같다.

$\mathbf{M}(\mathbf{q})\ddot{\mathbf{q}} + \mathbf{h}(\mathbf{q}, \dot{\mathbf{q}}) = \mathbf{S}^T\boldsymbol{\tau} + \mathbf{J}_c^T(\mathbf{q})\mathbf{f}_c$

여기서 $\mathbf{q} = [\mathbf{q}_b^T, \mathbf{q}_j^T]^T$ 는 부유 기저 좌표와 관절 좌표의 결합, $\mathbf{M}$ 은 일반화 관성 행렬, $\mathbf{h}$ 는 코리올리/원심/중력항의 결합, $\mathbf{S} = [\mathbf{0}, \mathbf{I}]$ 는 액추에이터 선택 행렬, $\boldsymbol{\tau}$ 는 관절 토크, $\mathbf{J}_c$ 는 접촉 야코비안, $\mathbf{f}_c$ 는 접촉력이다.

3. QP 기반 전신 제어의 정식화

3.1 결정 변수

전형적인 전신 제어 QP의 결정 변수는 다음과 같다.

$\mathbf{z} = [\ddot{\mathbf{q}}^T, \boldsymbol{\tau}^T, \mathbf{f}_c^T]^T$

관절 가속도, 관절 토크, 접촉력이 동시에 최적화된다.

3.2 비용 함수

다중 과업 목표를 가중 이차 비용으로 결합한다.

$J = \sum_{k=1}^{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 + w_f \lVert \mathbf{f}_c \rVert^2$

여기서 $\mathbf{J}_k$ 는 $k$ 번째 과업의 야코비안, $\ddot{\mathbf{x}}_{d,k}$ 는 원하는 과업 공간 가속도이다. 각 과업의 가중치 $w_k$ 는 과업의 상대적 중요도를 반영한다.

3.3 등식 제약

동역학 제약: 운동 방정식이 등식 제약으로 부과된다.

$\mathbf{M}\ddot{\mathbf{q}} + \mathbf{h} = \mathbf{S}^T\boldsymbol{\tau} + \mathbf{J}_c^T\mathbf{f}_c$

접촉 제약: 접촉점에서의 가속도가 영이어야 한다(접촉 유지 조건).

$\mathbf{J}_c\ddot{\mathbf{q}} + \dot{\mathbf{J}}_c\dot{\mathbf{q}} = \mathbf{0}$

3.4 부등식 제약

토크 한계:

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

관절 한계 (가속도 수준으로 변환):

$\ddot{q}_{i,\min} \leq \ddot{q}_i \leq \ddot{q}_{i,\max}$

마찰 원추 제약: 각 접촉점에서 접촉력이 마찰 원추 내에 있어야 한다.

$\sqrt{f_{x,j}^2 + f_{y,j}^2} \leq \mu f_{z,j}, \quad f_{z,j} \geq 0$

이 2차 원추 제약은 선형 다면체 근사(pyramidal approximation)로 변환하여 선형 부등식으로 처리하는 것이 일반적이다.

$\lvert f_{x,j} \rvert \leq \frac{\mu}{\sqrt{2}} f_{z,j}, \quad \lvert f_{y,j} \rvert \leq \frac{\mu}{\sqrt{2}} f_{z,j}$

4. 계층적 QP(Hierarchical QP)

다중 과업의 우선순위를 엄격하게 구분해야 하는 경우, 가중 합 대신 계층적(lexicographic) 최적화가 사용된다. 높은 우선순위의 과업이 낮은 우선순위의 과업에 의해 방해받지 않도록 보장한다.

우선순위 1 (최고): 동역학 일관성, 접촉 유지

우선순위 2: 무게 중심 제어, 자세 안정화

우선순위 3: 말단 장치 과업 추종

우선순위 4 (최저): 기본 자세 유지, 에너지 최소화

각 우선순위 수준에서 이전 수준의 최적해를 제약으로 추가하고 다음 수준의 QP를 풀어, 상위 과업의 성능을 보존하면서 하위 과업을 최적화한다.

5. 실시간 구현

전신 제어 QP의 규모는 로봇의 자유도와 접촉점 수에 의존한다. 30자유도 휴머노이드 로봇에서 결정 변수의 수는 약 100~200개, 제약의 수는 약 50~200개이다. 이 규모의 QP는 능동집합법(qpOASES) 또는 ADMM 기반 해법기(OSQP)에 의해 1ms 이내에 풀 수 있다.

웜 스타트가 실시간 성능의 핵심이며, 연속적인 제어 주기 사이의 능동 집합 변화가 소수에 그치므로 1~3회의 반복으로 수렴하는 경우가 일반적이다.

6. 대표적 프레임워크

전신 운동 제어를 위한 대표적 소프트웨어 프레임워크로 IHMC(Institute for Human and Machine Cognition)의 제어기, Pinocchio/Crocoddyl, Drake, TSID(Task-Space Inverse Dynamics) 등이 있다. 이 프레임워크들은 로봇 동역학 계산, QP 정식화, 실시간 해법을 통합적으로 제공한다.

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. (2015). Joint Position and Velocity Bounds in Discrete-Time Acceleration/Torque Control of Robot Manipulators. IEEE Robotics and Automation Letters.
Escande, A., Mansard, N., & Wieber, P.-B. (2014). “Hierarchical Quadratic Programming: Fast Online Humanoid-Robot Motion Generation.” The International Journal of Robotics Research, 33(7), 1006–1028.
Featherstone, R. (2008). Rigid Body Dynamics Algorithms. Springer.

version: 1.0