6.119 행렬 지수의 계산 방법
1. 개요
행렬 지수 e^{\mathbf{A}}의 수치적 계산은 수치 선형대수학에서 가장 도전적인 문제 중 하나이다. Moler와 Van Loan(1978, 2003)은 “행렬 지수를 계산하는 19가지 의심스러운 방법“이라는 유명한 논문에서 다양한 계산 방법의 장단점을 분석하였다. 로봇공학에서는 회전의 지수 표현, 동역학 시뮬레이션, 이산화된 제어 시스템의 상태 전이 행렬 등에서 행렬 지수의 계산이 필요하다. 본 절에서는 주요 계산 방법의 수학적 원리, 수치적 특성, 그리고 로봇공학에서의 적용을 다룬다.
2. 급수 합산법
2.1 테일러 급수
가장 직관적인 방법은 정의에 따라 급수를 직접 합산하는 것이다.
e^{\mathbf{A}} = \sum_{k=0}^{N} \frac{\mathbf{A}^k}{k!} + O\left(\frac{\|\mathbf{A}\|^{N+1}}{(N+1)!}\right)
절단 오차는 \|\mathbf{A}\|가 작을 때 급격히 감소한다. 그러나 이 방법은 다음과 같은 문제가 있다.
- \|\mathbf{A}\|가 크면 많은 항이 필요하다
- 중간 계산에서 큰 값이 나타났다가 소거되는 경우 심각한 반올림 오차가 발생한다
- \mathbf{A}의 고유값이 큰 음의 실수부를 가지면, e^{\mathbf{A}}는 작은 값인데 급수의 개별 항은 클 수 있다
2.2 호너 방법
급수의 효율적인 평가를 위해 호너(Horner) 방법을 적용할 수 있다.
\mathbf{S}_N = \mathbf{I} + \mathbf{A}\left(\frac{1}{1!}\mathbf{I} + \mathbf{A}\left(\frac{1}{2!}\mathbf{I} + \mathbf{A}\left(\cdots + \frac{1}{N!}\mathbf{A}\right)\right)\right)
이 방법은 행렬 곱셈 횟수를 N회로 줄이지만, 수치적 문제는 해결하지 못한다.
3. 파데 근사법
3.1 파데 근사의 정의
[p/q] 파데 근사(Padé approximation)는 e^x를 유리 함수로 근사하는 방법이다.
e^{\mathbf{A}} \approx R_{pq}(\mathbf{A}) = [D_{pq}(\mathbf{A})]^{-1} N_{pq}(\mathbf{A})
여기서 분자와 분모 다항식은 다음과 같다.
N_{pq}(\mathbf{A}) = \sum_{j=0}^{p} \frac{(p+q-j)!\, p!}{(p+q)!\, j!\, (p-j)!}\, \mathbf{A}^j
D_{pq}(\mathbf{A}) = \sum_{j=0}^{q} \frac{(p+q-j)!\, q!}{(p+q)!\, j!\, (q-j)!}\, (-\mathbf{A})^j
파데 근사의 오차는 다음과 같다.
e^{\mathbf{A}} - R_{pq}(\mathbf{A}) = O(\|\mathbf{A}\|^{p+q+1})
3.2 대각 파데 근사
p = q인 대각 파데 근사(diagonal Padé approximation)가 가장 널리 사용된다. [q/q] 파데 근사에서 분자와 분모는 다음과 같이 단순화된다.
N_{qq}(\mathbf{A}) = \sum_{j=0}^{q} c_j \mathbf{A}^j, \quad D_{qq}(\mathbf{A}) = \sum_{j=0}^{q} c_j (-\mathbf{A})^j
여기서 계수는 다음과 같다.
c_j = \frac{(2q - j)!\, q!}{(2q)!\, j!\, (q - j)!}
대각 파데 근사는 다음과 같은 중요한 성질을 갖는다.
- D_{qq}(\mathbf{A}) = N_{qq}(-\mathbf{A})이므로 분모를 별도로 계산할 필요가 없다
- \mathbf{A}가 반대칭이면 R_{qq}(\mathbf{A})는 직교 행렬이다 (직교성 보존)
| 차수 q | 오차 차수 | 행렬 곱 횟수 | 정밀도 (배정밀도 기준, \lVert\mathbf{A}\rVert \leq 1) |
|---|---|---|---|
| 3 | O(\lVert\mathbf{A}\rVert^7) | 4 | 약 10^{-7} |
| 5 | O(\lVert\mathbf{A}\rVert^{11}) | 6 | 약 10^{-11} |
| 7 | O(\lVert\mathbf{A}\rVert^{15}) | 8 | 약 10^{-15} |
| 13 | O(\lVert\mathbf{A}\rVert^{27}) | 12 | 약 10^{-16} |
4. 스케일링-제곱 방법
4.1 알고리즘 원리
스케일링-제곱(scaling and squaring) 방법은 다음 항등식에 기반한다.
e^{\mathbf{A}} = \left(e^{\mathbf{A}/2^s}\right)^{2^s}
정수 s를 적절히 선택하여 \|\mathbf{A}/2^s\|를 충분히 작게 만든 후, e^{\mathbf{A}/2^s}를 파데 근사로 정확하게 계산하고, s번의 반복 제곱으로 e^{\mathbf{A}}를 얻는다.
4.2 스케일링 인수의 선택
스케일링 인수 s는 다음 조건을 만족하도록 선택한다.
s = \max\left(0, \left\lceil \log_2 \frac{\|\mathbf{A}\|}{\theta_q} \right\rceil\right)
여기서 \theta_q는 [q/q] 파데 근사의 오차가 단위 반올림 수준이 되는 임계값이다. 배정밀도에서 [13/13] 파데 근사를 사용할 때 \theta_{13} \approx 5.37이다.
4.3 전체 알고리즘
스케일링-제곱 알고리즘은 다음과 같다.
- s = \max(0, \lceil \log_2(\|\mathbf{A}\|_1 / \theta_{13}) \rceil)를 계산한다
- \mathbf{B} = \mathbf{A}/2^s를 계산한다
- \mathbf{B}^2, \mathbf{B}^4, \mathbf{B}^6을 계산한다
- N_{13,13}(\mathbf{B})와 D_{13,13}(\mathbf{B})를 계산한다
- D_{13,13}(\mathbf{B})\, \mathbf{X} = N_{13,13}(\mathbf{B})를 LU 분해로 풀어 \mathbf{X} \approx e^{\mathbf{B}}를 얻는다
- \mathbf{X} \leftarrow \mathbf{X}^2를 s번 반복하여 e^{\mathbf{A}}를 얻는다
이 알고리즘은 MATLAB의 expm 함수와 SciPy의 scipy.linalg.expm 함수에서 사용되는 표준 방법이다.
5. 고유값 분해 기반 방법
5.1 대각화 가능한 경우
\mathbf{A}가 대각화 가능하여 \mathbf{A} = \mathbf{P}\boldsymbol{\Lambda}\mathbf{P}^{-1}이면, 다음과 같이 계산한다.
e^{\mathbf{A}} = \mathbf{P}\, \text{diag}(e^{\lambda_1}, e^{\lambda_2}, \ldots, e^{\lambda_n})\, \mathbf{P}^{-1}
이 방법은 개념적으로 단순하지만, 다음과 같은 수치적 문제가 있다.
- \mathbf{P}의 조건수 \kappa(\mathbf{P})가 클 때 \mathbf{P}^{-1}의 계산이 불안정하다
- 고유값이 거의 중복(defective)인 경우 \mathbf{P}가 비정칙해진다
- 전체 오차가 O(\kappa(\mathbf{P}) \cdot u)로 증폭된다
5.2 슈어 분해 기반 방법
수치적으로 더 안정적인 방법은 슈어 분해(Schur decomposition)를 사용하는 것이다.
\mathbf{A} = \mathbf{Q}\mathbf{T}\mathbf{Q}^H
여기서 \mathbf{Q}는 유니터리 행렬이고 \mathbf{T}는 상삼각 행렬이다. 그러면 다음과 같다.
e^{\mathbf{A}} = \mathbf{Q}\, e^{\mathbf{T}}\, \mathbf{Q}^H
상삼각 행렬의 지수 e^{\mathbf{T}}는 대각 원소가 e^{t_{ii}}이고, 비대각 원소는 파레(Parlett)의 재귀 공식으로 계산한다.
(e^{\mathbf{T}})_{ij} = \begin{cases} e^{t_{ii}} & i = j \\ \displaystyle\frac{(e^{\mathbf{T}})_{ij} \cdot (t_{jj} - t_{ii}) + \sum_{k=i+1}^{j-1} (e^{\mathbf{T}})_{ik}\, t_{kj} - t_{ik}\, (e^{\mathbf{T}})_{kj}}{t_{jj} - t_{ii}} & i < j,\, t_{ii} \neq t_{jj} \end{cases}
유니터리 행렬의 조건수가 1이므로(\kappa_2(\mathbf{Q}) = 1), 이 방법은 고유값 분해 방법보다 수치적으로 안정적이다.
6. 크릴로프 부분공간 방법
6.1 대규모 행렬에 대한 접근
로봇의 유한 요소 모델이나 다체 동역학 시뮬레이션에서는 대규모 희소 행렬(large sparse matrix)의 지수를 계산해야 하는 경우가 있다. 이 경우 행렬 지수 자체보다는 행렬 지수와 벡터의 곱 e^{\mathbf{A}}\mathbf{b}를 효율적으로 계산하는 것이 목표이다.
크릴로프 부분공간(Krylov subspace) 방법은 다음과 같은 m차 크릴로프 부분공간에서 근사를 구한다.
\mathcal{K}_m(\mathbf{A}, \mathbf{b}) = \text{span}\{\mathbf{b}, \mathbf{A}\mathbf{b}, \mathbf{A}^2\mathbf{b}, \ldots, \mathbf{A}^{m-1}\mathbf{b}\}
6.2 아놀디(Arnoldi) 과정
아놀디 과정을 통해 크릴로프 부분공간의 직교 정규 기저 \mathbf{V}_m = [\mathbf{v}_1, \mathbf{v}_2, \ldots, \mathbf{v}_m]과 상헤센베르크 행렬 \mathbf{H}_m을 구한다.
\mathbf{A}\mathbf{V}_m = \mathbf{V}_m\mathbf{H}_m + h_{m+1,m}\,\mathbf{v}_{m+1}\mathbf{e}_m^T
행렬 지수-벡터 곱의 근사는 다음과 같다.
e^{\mathbf{A}}\mathbf{b} \approx \|\mathbf{b}\|\, \mathbf{V}_m\, e^{\mathbf{H}_m}\, \mathbf{e}_1
\mathbf{H}_m은 m \times m의 작은 행렬이므로 e^{\mathbf{H}_m}은 스케일링-제곱 방법으로 효율적으로 계산할 수 있다. 원래 행렬이 n \times n (n \gg m)이면, 계산 비용이 O(nm)의 행렬-벡터 곱과 O(m^3)의 소규모 행렬 지수 계산으로 대폭 절감된다.
7. 로봇공학의 특수한 경우
7.1 SO(3)에서의 행렬 지수: 로드리게스 공식
3 \times 3 반대칭 행렬 [\boldsymbol{\omega}]_\times에 대한 행렬 지수는 로드리게스(Rodrigues) 공식으로 닫힌 형태(closed form)를 갖는다.
e^{[\boldsymbol{\omega}]_\times} = \mathbf{I} + \frac{\sin\theta}{\theta}[\boldsymbol{\omega}]_\times + \frac{1 - \cos\theta}{\theta^2}[\boldsymbol{\omega}]_\times^2
여기서 \theta = \|\boldsymbol{\omega}\|이다. 이 공식은 반대칭 행렬이 [\boldsymbol{\omega}]_\times^3 = -\theta^2 [\boldsymbol{\omega}]_\times를 만족한다는 성질(Cayley-Hamilton 정리)로부터 유도된다.
\theta \approx 0일 때는 다음과 같은 테일러 전개를 사용하여 수치적 안정성을 확보한다.
\frac{\sin\theta}{\theta} \approx 1 - \frac{\theta^2}{6} + \frac{\theta^4}{120}
\frac{1 - \cos\theta}{\theta^2} \approx \frac{1}{2} - \frac{\theta^2}{24} + \frac{\theta^4}{720}
7.2 SE(3)에서의 행렬 지수
4 \times 4 트위스트 행렬 [\boldsymbol{\xi}] \in \mathfrak{se}(3)에 대한 행렬 지수도 닫힌 형태를 갖는다.
[\boldsymbol{\xi}] = \begin{bmatrix} [\boldsymbol{\omega}]_\times & \mathbf{v} \\ \mathbf{0}^T & 0 \end{bmatrix}
에 대해,
e^{[\boldsymbol{\xi}]} = \begin{bmatrix} e^{[\boldsymbol{\omega}]_\times} & \mathbf{G}\mathbf{v} \\ \mathbf{0}^T & 1 \end{bmatrix}
여기서 \mathbf{G}는 다음과 같다.
\mathbf{G} = \mathbf{I} + \frac{1 - \cos\theta}{\theta^2}[\boldsymbol{\omega}]_\times + \frac{\theta - \sin\theta}{\theta^3}[\boldsymbol{\omega}]_\times^2
\boldsymbol{\omega} = \mathbf{0}인 순수 병진 운동의 경우에는 단순히 다음과 같다.
e^{[\boldsymbol{\xi}]} = \begin{bmatrix} \mathbf{I} & \mathbf{v} \\ \mathbf{0}^T & 1 \end{bmatrix}
7.3 계산 방법의 선택 지침
로봇공학에서 행렬 지수 계산 방법의 선택은 다음 기준에 따른다.
| 상황 | 권장 방법 | 이유 |
|---|---|---|
| 3 \times 3 반대칭 행렬 | 로드리게스 공식 | 닫힌 형태, O(1) |
| 4 \times 4 트위스트 행렬 | SE(3) 닫힌 공식 | 닫힌 형태, O(1) |
| 소규모 일반 행렬 (n \leq 100) | 스케일링-제곱 + 파데 | 범용적, 안정적 |
| 대규모 희소 행렬 | 크릴로프 부분공간 | 효율적, 행렬-벡터 곱만 필요 |
8. 수치적 안정성 비교
다양한 계산 방법의 수치적 안정성을 비교한다. 전진 오차(forward error) \|e^{\mathbf{A}} - \hat{e}^{\mathbf{A}}\| / \|e^{\mathbf{A}}\|를 기준으로 평가한다.
테일러 급수: \|\mathbf{A}\|가 크거나 고유값의 실수부가 큰 음수일 때 심각한 소거 오차가 발생한다. 일반적으로 권장하지 않는다.
고유값 분해: \kappa(\mathbf{P})에 비례하는 오차가 발생한다. 정규 행렬(normal matrix, \mathbf{A}^H\mathbf{A} = \mathbf{A}\mathbf{A}^H)에서는 \kappa(\mathbf{P}) = 1이므로 안정적이다.
스케일링-제곱 + 파데: 후진 안정(backward stable)하며, 오차가 \|\mathbf{A}\|에 대해 온건하게 증가한다. 가장 범용적으로 신뢰할 수 있다.
슈어 분해 기반: 유니터리 변환을 사용하므로 고유값 분해보다 안정적이다. 고유값이 근접한 경우에도 잘 동작한다.
9. 참고 문헌
- Moler, C., & Van Loan, C. (2003). “Nineteen Dubious Ways to Compute the Exponential of a Matrix, Twenty-Five Years Later.” SIAM Review, 45(1), 3–49.
- Higham, N. J. (2005). “The Scaling and Squaring Method for the Matrix Exponential Revisited.” SIAM Journal on Matrix Analysis and Applications, 26(4), 1179–1193.
- Higham, N. J. (2008). Functions of Matrices: Theory and Computation. SIAM.
- Murray, R. M., Li, Z., & Sastry, S. S. (1994). A Mathematical Introduction to Robotic Manipulation. CRC Press.
- Lynch, K. M., & Park, F. C. (2017). Modern Robotics: Mechanics, Planning, and Control. Cambridge University Press.
- Golub, G. H., & Van Loan, C. F. (2013). Matrix Computations (4th ed.). Johns Hopkins University Press.
- Saad, Y. (1992). “Analysis of Some Krylov Subspace Approximations to the Matrix Exponential Operator.” SIAM Journal on Numerical Analysis, 29(1), 209–228.
v 0.1