6.136 최소 에너지 경로 계획과 선형 제약 조건

1. 최소 에너지 문제의 정의

로봇의 경로 계획에서 에너지 소비를 최소화하는 것은 배터리 수명이 제한된 이동 로봇이나 에너지 효율이 중요한 산업용 로봇에서 핵심적인 문제이다. 최소 에너지 경로 계획 문제는 다음과 같이 정식화한다.

\min_{\mathbf{u}(t)} \quad J = \int_{t_0}^{t_f} \mathbf{u}(t)^T \mathbf{R} \, \mathbf{u}(t) \, dt

\text{subject to} \quad \dot{\mathbf{x}}(t) = \mathbf{A}\mathbf{x}(t) + \mathbf{B}\mathbf{u}(t), \quad \mathbf{x}(t_0) = \mathbf{x}_0, \quad \mathbf{x}(t_f) = \mathbf{x}_f

여기서 \mathbf{x}(t) \in \mathbb{R}^n은 상태 벡터, \mathbf{u}(t) \in \mathbb{R}^m은 제어 입력, \mathbf{R} \in \mathbb{R}^{m \times m}은 양의 정치(positive definite) 가중 행렬이다. 비용 함수 J는 제어 입력의 제곱 노름에 비례하며, 이는 관절 토크의 제곱에 비례하는 에너지 소비를 근사한다.

2. 해석적 해: 그라미안을 이용한 풀이

2.1 제어 가능성 그라미안

선형 시간 불변(LTI) 시스템에서, 상태 전이 행렬 \boldsymbol{\Phi}(t, t_0) = e^{\mathbf{A}(t - t_0)}를 사용하면 상태 응답은 다음과 같다.

\mathbf{x}(t_f) = e^{\mathbf{A}(t_f - t_0)} \mathbf{x}_0 + \int_{t_0}^{t_f} e^{\mathbf{A}(t_f - \tau)} \mathbf{B} \mathbf{u}(\tau) \, d\tau

경계 조건 \mathbf{x}(t_f) = \mathbf{x}_f를 만족하는 최소 에너지 제어 입력은 제어 가능성 그라미안(controllability Gramian)을 이용하여 구한다.

\mathbf{W}_c = \int_{t_0}^{t_f} e^{\mathbf{A}(t_f - \tau)} \mathbf{B} \mathbf{R}^{-1} \mathbf{B}^T e^{\mathbf{A}^T(t_f - \tau)} \, d\tau

가중 행렬 \mathbf{R} = \mathbf{I}인 경우, 이는 표준 제어 가능성 그라미안과 일치한다.

2.2 최적 제어 입력

최소 에너지 제어 입력은 다음과 같이 표현된다.

\mathbf{u}^*(t) = \mathbf{R}^{-1} \mathbf{B}^T e^{\mathbf{A}^T(t_f - t)} \mathbf{W}_c^{-1} \bigl(\mathbf{x}_f - e^{\mathbf{A}(t_f - t_0)} \mathbf{x}_0\bigr)

최소 에너지 값은 다음과 같다.

J^* = \bigl(\mathbf{x}_f - e^{\mathbf{A}(t_f - t_0)} \mathbf{x}_0\bigr)^T \mathbf{W}_c^{-1} \bigl(\mathbf{x}_f - e^{\mathbf{A}(t_f - t_0)} \mathbf{x}_0\bigr)

이 결과는 \mathbf{W}_c가 가역(invertible)일 때, 즉 시스템이 제어 가능(controllable)할 때만 유효하다. \mathbf{W}_c의 고유값이 작은 방향은 많은 에너지를 필요로 하는 상태 전이 방향에 해당한다.

3. 이산화된 최소 에너지 문제

3.1 이산 시간 정식화

시간을 N개의 구간으로 이산화하면 다음의 이차 계획(QP) 문제를 얻는다.

\begin{aligned} \min_{\mathbf{u}_0, \dots, \mathbf{u}_{N-1}} \quad & \sum_{k=0}^{N-1} \mathbf{u}_k^T \mathbf{R} \, \mathbf{u}_k \\ \text{subject to} \quad & \mathbf{x}_{k+1} = \mathbf{A}_d \mathbf{x}_k + \mathbf{B}_d \mathbf{u}_k, \quad k = 0, \dots, N-1 \\ & \mathbf{x}_0 = \mathbf{x}_{\text{init}}, \quad \mathbf{x}_N = \mathbf{x}_{\text{goal}} \end{aligned}

여기서 \mathbf{A}_d = e^{\mathbf{A}h}, \mathbf{B}_d = \int_0^h e^{\mathbf{A}\tau} \mathbf{B} \, d\tau는 이산화된 시스템 행렬이다.

3.2 행렬 형태의 정식화

제어 입력 벡터를 쌓으면 \mathbf{U} = [\mathbf{u}_0^T, \mathbf{u}_1^T, \dots, \mathbf{u}_{N-1}^T]^T \in \mathbb{R}^{Nm}이 된다. 역학 제약을 반복적으로 대입하면 최종 상태를 다음과 같이 표현할 수 있다.

\mathbf{x}_N = \mathbf{A}_d^N \mathbf{x}_0 + \begin{bmatrix} \mathbf{A}_d^{N-1}\mathbf{B}_d & \mathbf{A}_d^{N-2}\mathbf{B}_d & \cdots & \mathbf{B}_d \end{bmatrix} \mathbf{U}

이를 간결하게 쓰면

\mathbf{x}_N = \mathbf{A}_d^N \mathbf{x}_0 + \mathbf{G} \mathbf{U}

여기서 \mathbf{G} \in \mathbb{R}^{n \times Nm}은 제어 가능성 행렬(controllability matrix)의 일반화이다. 비용 함수는 다음과 같이 표현된다.

J = \mathbf{U}^T (\mathbf{I}_N \otimes \mathbf{R}) \mathbf{U} = \mathbf{U}^T \bar{\mathbf{R}} \, \mathbf{U}

여기서 \otimes는 크로네커 곱(Kronecker product)이고, \bar{\mathbf{R}} = \text{blkdiag}(\mathbf{R}, \mathbf{R}, \dots, \mathbf{R})이다.

3.3 등식 제약 QP의 해

등식 제약 \mathbf{G}\mathbf{U} = \mathbf{x}_f - \mathbf{A}_d^N \mathbf{x}_0 \equiv \mathbf{d}를 가지는 이차 계획 문제의 KKT 조건은 다음과 같다.

$$
\begin{bmatrix}
2\bar{\mathbf{R}} & \mathbf{G}^{T \
\mathbf{G} & \mathbf{0}
\end{bmatrix}
\begin{bmatrix}
\mathbf{U}}* \
\boldsymbol{\lambda}^*
\end

\begin{bmatrix}
\mathbf{0} \
\mathbf{d}
\end{bmatrix}
$$

이로부터 최적해는 다음과 같다.

\mathbf{U}^* = \bar{\mathbf{R}}^{-1} \mathbf{G}^T (\mathbf{G} \bar{\mathbf{R}}^{-1} \mathbf{G}^T)^{-1} \mathbf{d}

이 해는 연속 시간 해의 이산화 버전에 해당하며, \mathbf{G} \bar{\mathbf{R}}^{-1} \mathbf{G}^T는 이산 제어 가능성 그라미안에 해당한다.

4. 선형 부등식 제약 조건

4.1 관절 한계와 토크 한계

실제 로봇에서는 관절 각도, 각속도, 토크에 물리적 한계가 존재한다. 이를 선형 부등식 제약으로 표현한다.

\mathbf{q}_{\min} \leq \mathbf{q}_k \leq \mathbf{q}_{\max}, \quad k = 0, 1, \dots, N

\boldsymbol{\tau}_{\min} \leq \mathbf{u}_k \leq \boldsymbol{\tau}_{\max}, \quad k = 0, 1, \dots, N-1

이를 행렬 형태로 정리하면

\mathbf{C}\mathbf{z} \leq \mathbf{d}

여기서 \mathbf{z}는 모든 결정 변수를 포함하는 벡터이고, \mathbf{C}는 적절한 선택 행렬과 부호를 결합한 희소 행렬이다.

4.2 장애물 회피의 선형 근사

장애물 회피 조건은 본질적으로 비볼록이지만, 현재 궤적을 기준으로 선형화할 수 있다. 장애물 표면까지의 부호 거리 함수(signed distance function)를 d(\mathbf{x})라 하면, 현재 점 \bar{\mathbf{x}}_k 근방에서 1차 테일러 전개를 적용한다.

d(\mathbf{x}_k) \approx d(\bar{\mathbf{x}}_k) + \nabla d(\bar{\mathbf{x}}_k)^T (\mathbf{x}_k - \bar{\mathbf{x}}_k) \geq d_{\text{safe}}

이를 정리하면 선형 부등식 제약이 된다.

\nabla d(\bar{\mathbf{x}}_k)^T \mathbf{x}_k \geq d_{\text{safe}} - d(\bar{\mathbf{x}}_k) + \nabla d(\bar{\mathbf{x}}_k)^T \bar{\mathbf{x}}_k

이러한 선형 근사를 반복적으로 갱신하는 순차 볼록 계획(Sequential Convex Programming, SCP)을 통하여 비볼록 장애물 회피 문제를 해결할 수 있다.

5. 제약 조건이 있는 QP의 풀이

5.1 능동 집합법(Active Set Method)

부등식 제약이 포함된 QP에서, 최적해에서 등호로 성립하는 제약의 집합을 능동 집합(active set)이라 한다. 능동 집합법은 능동 집합을 추측하고, 해당 제약만 등식으로 바꾼 축소 문제를 풀어 능동 집합을 갱신하는 과정을 반복한다.

능동 집합 \mathcal{A}가 주어지면, 축소 KKT 시스템은 다음과 같다.

$$
\begin{bmatrix}
\mathbf{H} & \mathbf{A}{\mathcal{A}}^T \
\mathbf{A}{\mathcal{A}} & \mathbf{0}
\end{bmatrix}
\begin{bmatrix}
\Delta \mathbf{z} \
\boldsymbol{\lambda}_{\mathcal{A}}
\end

\begin{bmatrix}
-\mathbf{g} \
\mathbf{0}
\end{bmatrix}
$$

여기서 \mathbf{A}_{\mathcal{A}}는 능동 제약에 해당하는 행을 추출한 행렬이다.

5.2 내점법

내점법은 부등식 제약을 로그 장벽(log-barrier) 함수로 대체하여 등식 제약 문제의 연속으로 변환한다.

\min_{\mathbf{z}} \quad \frac{1}{2}\mathbf{z}^T \mathbf{H}\mathbf{z} + \mathbf{g}^T \mathbf{z} - \frac{1}{t} \sum_{i} \log(d_i - \mathbf{c}_i^T \mathbf{z})

장벽 파라미터 t를 점진적으로 증가시키면서 뉴턴법으로 풀면, 해가 중심 경로(central path)를 따라 최적해에 수렴한다. 궤적 최적화에서 제약 행렬의 희소 구조를 이용하면 뉴턴 스텝의 계산을 효율적으로 수행할 수 있다.

6. 경유점 제약과 웨이포인트

로봇이 특정 중간 지점(waypoint)을 통과해야 하는 조건은 등식 제약으로 표현한다.

\mathbf{x}_{k_j} = \mathbf{w}_j, \quad j = 1, 2, \dots, p

여기서 k_j는 j번째 경유점에 해당하는 시간 인덱스이고, \mathbf{w}_j는 원하는 상태이다. 이 제약을 포함하면 전체 문제는 여전히 QP이며, 선형 등식 제약의 수만 증가한다.

경유점 제약이 많아지면, 행렬 \mathbf{G}의 행이 추가되어 제약 행렬의 크기가 증가한다. 이 경우 블록 구조를 활용한 효율적인 분해가 중요하다.

7. 다관절 로봇에서의 에너지 최적화

n_q개의 관절을 가진 로봇에서 관절 토크 벡터 \boldsymbol{\tau} \in \mathbb{R}^{n_q}에 대한 에너지 소비는 다음과 같이 모델링한다.

E = \int_{t_0}^{t_f} \boldsymbol{\tau}(t)^T \boldsymbol{\tau}(t) \, dt \approx h \sum_{k=0}^{N-1} \boldsymbol{\tau}_k^T \boldsymbol{\tau}_k

역동역학(inverse dynamics) \boldsymbol{\tau}_k = \mathbf{M}(\mathbf{q}_k)\ddot{\mathbf{q}}_k + \mathbf{c}(\mathbf{q}_k, \dot{\mathbf{q}}_k) + \mathbf{g}(\mathbf{q}_k)를 대입하면, \boldsymbol{\tau}_k는 \mathbf{q}_k, \dot{\mathbf{q}}_k, \ddot{\mathbf{q}}_k의 비선형 함수가 된다. 현재 궤적 근방에서 선형화하면 다음 형태의 QP 부분 문제를 반복적으로 풀 수 있다.

\begin{aligned} \min_{\Delta \mathbf{q}} \quad & \Delta \mathbf{q}^T \mathbf{H}_{\text{energy}} \Delta \mathbf{q} + \mathbf{g}_{\text{energy}}^T \Delta \mathbf{q} \\ \text{subject to} \quad & \mathbf{A}_{\text{eq}} \Delta \mathbf{q} = \mathbf{b}_{\text{eq}} \\ & \mathbf{A}_{\text{ineq}} \Delta \mathbf{q} \leq \mathbf{b}_{\text{ineq}} \end{aligned}

8. 참고 문헌

• Kirk, D. E. (2004). Optimal Control Theory: An Introduction. Dover Publications.
• Boyd, S., & Vandenberghe, L. (2004). Convex Optimization. Cambridge University Press.
• Betts, J. T. (2010). Practical Methods for Optimal Control and Estimation Using Nonlinear Programming (2nd ed.). SIAM.
• Siciliano, B., Sciavicco, L., Villani, L., & Oriolo, G. (2009). Robotics: Modelling, Planning and Control. Springer.
• Schulman, J., Duan, Y., Ho, J., Lee, A., Awwal, I., Bradlow, H., Pan, J., Patil, S., Goldberg, K., & Abbeel, P. (2014). Motion planning with sequential convex optimization and convex collision checking. International Journal of Robotics Research, 33(9), 1251–1270.

v 0.1