6.117 로봇 제어에서의 수치적 안정성 확보 전략

1. 개요

로봇 제어 시스템은 실시간으로 기구학, 동역학, 경로 계획 등 다양한 수치 계산을 수행한다. 이러한 계산에서 수치적 불안정성은 로봇의 비정상적인 거동, 과도한 관절 토크, 또는 제어 루프의 발산으로 이어질 수 있다. 본 절에서는 로봇 제어에서 발생하는 수치적 불안정성의 원인을 체계적으로 분류하고, 이를 극복하기 위한 실용적인 전략을 선형대수학의 관점에서 제시한다.

2. 수치적 불안정성의 원인 분류

로봇 제어에서 수치적 불안정성의 원인은 크게 네 가지로 분류할 수 있다.

행렬 비정칙성(Ill-conditioning): 야코비 행렬이나 관성 행렬의 조건수가 매우 클 때, 입력의 미세한 오차가 출력에서 크게 증폭된다.

특이점(Singularity): 야코비 행렬의 랭크가 감소하여 역행렬이 존재하지 않는 배치에서, 수치 계산이 발산한다.

이산화 오차(Discretization error): 연속 시간 제어 법칙을 이산 시간으로 구현할 때, 샘플링 주기와 수치 적분 방법에 의해 오차가 발생한다.

반올림 오차 누적(Accumulated rounding error): 반복적인 부동 소수점 연산에서 오차가 점진적으로 누적된다.

3. 역기구학의 수치적 안정성

3.1 유사역행렬 기반 방법의 문제점

역기구학에서 관절 속도를 구하는 기본 방정식은 다음과 같다.

\dot{\mathbf{q}} = \mathbf{J}^{\dagger}(\mathbf{q})\,\dot{\mathbf{x}}

여기서 \mathbf{J}^{\dagger}는 무어-펜로즈 유사역행렬(Moore-Penrose pseudoinverse)이다. 특이점 근방에서 \sigma_{\min} \to 0이므로, 유사역행렬의 원소가 무한대로 발산하여 \|\dot{\mathbf{q}}\| \to \infty가 된다.

3.2 감쇠 최소 제곱법

감쇠 최소 제곱법(Damped Least Squares, DLS)은 특이점에서의 수치적 안정성을 확보하는 가장 널리 사용되는 방법이다.

\dot{\mathbf{q}} = \mathbf{J}^T(\mathbf{J}\mathbf{J}^T + \lambda^2\mathbf{I})^{-1}\dot{\mathbf{x}}

감쇠 인수 \lambda의 적응적 설정은 다음과 같이 조작성 지표에 기반할 수 있다.

\lambda^2 = \begin{cases} 0 & \text{if } w \geq w_0 \\ \lambda_{\max}^2\left(1 - \left(\frac{w}{w_0}\right)^2\right) & \text{if } w < w_0 \end{cases}

여기서 w = \sqrt{\det(\mathbf{J}\mathbf{J}^T)}는 조작성 지표이고, w_0는 임계값이다.

3.3 선택적 감쇠 역행렬

선택적 감쇠(Selectively Damped Least Squares, SDLS)는 각 작업 공간 방향에 대해 독립적인 감쇠를 적용한다. SVD \mathbf{J} = \mathbf{U}\boldsymbol{\Sigma}\mathbf{V}^T를 이용하여 다음과 같이 계산한다.

\dot{\mathbf{q}} = \sum_{i=1}^{m} \frac{\sigma_i}{\sigma_i^2 + \lambda_i^2}(\mathbf{u}_i^T\dot{\mathbf{x}})\,\mathbf{v}_i

각 방향의 감쇠 인수 \lambda_i는 해당 방향의 특이값과 원하는 관절 속도의 크기를 고려하여 설정한다. Buss와 Kim(2005)이 제안한 방법에서는 다음과 같이 설정한다.

\lambda_i = \begin{cases} 0 & \text{if } \sigma_i \geq \sigma_{\text{thresh}} \\ \frac{\gamma_i}{\sigma_i} & \text{if } \sigma_i < \sigma_{\text{thresh}} \end{cases}

여기서 \gamma_i는 각 작업 공간 방향에서의 최대 허용 관절 속도에 의해 결정되는 감쇠 상한이다.

4. 관성 행렬의 수치적 처리

4.1 양의 정부호성 보장

로봇 동역학 방정식은 다음과 같다.

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

관성 행렬 \mathbf{M}(\mathbf{q})는 이론적으로 양의 정부호(positive definite) 대칭 행렬이다. 그러나 수치 계산에서는 반올림 오차에 의해 양의 정부호성이 미세하게 위반될 수 있다. 이를 방지하기 위해 다음과 같은 전략을 사용한다.

촐레스키 분해 기반 검증: \mathbf{M} = \mathbf{L}\mathbf{L}^T 형태의 촐레스키 분해가 성공하면 양의 정부호성이 보장된다. 실패하면 대각 보정을 적용한다.

\mathbf{M}_{\text{corrected}} = \mathbf{M} + \epsilon\mathbf{I}

여기서 \epsilon > 0은 촐레스키 분해가 성공하는 최소값으로 설정한다.

고유값 클리핑: \mathbf{M}의 고유값 분해 \mathbf{M} = \mathbf{Q}\boldsymbol{\Lambda}\mathbf{Q}^T에서, 음수이거나 매우 작은 고유값을 양의 최솟값으로 대체한다.

\tilde{\lambda}_i = \max(\lambda_i, \epsilon)

4.2 효율적인 선형 시스템 풀이

동역학 시뮬레이션에서 \mathbf{M}(\mathbf{q})\ddot{\mathbf{q}} = \boldsymbol{\tau} - \mathbf{C}\dot{\mathbf{q}} - \mathbf{g}를 풀 때, 역행렬 \mathbf{M}^{-1}을 명시적으로 계산하지 않고 분해 기반 풀이를 사용하는 것이 수치적으로 안정하다.

관성 행렬이 양의 정부호이므로, 촐레스키 분해가 가장 효율적이면서 안정적인 선택이다.

5. 시간 적분의 수치적 안정성

5.1 강성 시스템과 적분 방법

로봇 동역학 시뮬레이션에서 시간 적분은 핵심적인 수치 계산이다. 시스템이 강성(stiff)인 경우, 즉 고유값의 비율이 매우 큰 경우 명시적(explicit) 적분 방법은 불안정해질 수 있다.

상미분 방정식 \dot{\mathbf{y}} = \mathbf{f}(\mathbf{y}, t)에 대해, 야코비 행렬 \partial\mathbf{f}/\partial\mathbf{y}의 고유값이 넓은 범위에 걸쳐 있으면 강성 시스템이다. 강성비(stiffness ratio)는 다음과 같다.

\rho = \frac{\max_i \vert\text{Re}(\lambda_i)\vert}{\min_i \vert\text{Re}(\lambda_i)\vert}

\rho가 크면 시스템은 강성이다.

5.2 명시적 방법과 암묵적 방법

명시적 오일러(Explicit Euler):

\mathbf{y}_{k+1} = \mathbf{y}_k + h\,\mathbf{f}(\mathbf{y}_k, t_k)

안정 조건: h < 2/\vert\lambda_{\max}\vert. 강성 시스템에서는 h가 매우 작아야 하므로 비효율적이다.

암묵적 오일러(Implicit Euler):

\mathbf{y}_{k+1} = \mathbf{y}_k + h\,\mathbf{f}(\mathbf{y}_{k+1}, t_{k+1})

무조건 안정(unconditionally stable)하여 강성 시스템에 적합하지만, 매 스텝마다 비선형 방정식을 풀어야 한다.

반암묵적 방법(Semi-implicit methods): 실시간 로봇 제어에서는 계산 효율과 안정성의 균형을 위해 반암묵적 방법을 사용한다. 대표적으로 심플렉틱 오일러(symplectic Euler) 방법이 있다.

\begin{aligned} \dot{\mathbf{q}}_{k+1} &= \dot{\mathbf{q}}_k + h\,\mathbf{M}^{-1}(\boldsymbol{\tau} - \mathbf{C}\dot{\mathbf{q}}_k - \mathbf{g}) \\ \mathbf{q}_{k+1} &= \mathbf{q}_k + h\,\dot{\mathbf{q}}_{k+1} \end{aligned}

이 방법은 에너지 보존 특성이 우수하여 장시간 시뮬레이션에 적합하다.

5.3 적분 스텝 크기 제어

적응적 스텝 크기 제어는 수치적 안정성을 보장하면서 계산 효율을 최대화한다. 국소 절단 오차(local truncation error) \mathbf{e}_k를 추정하고, 다음 스텝 크기를 다음과 같이 조절한다.

h_{k+1} = h_k \cdot \min\left(\alpha_{\max},\, \max\left(\alpha_{\min},\, \beta \left(\frac{\text{tol}}{\|\mathbf{e}_k\|}\right)^{1/(p+1)}\right)\right)

여기서 p는 적분 방법의 차수, \text{tol}은 허용 오차, \beta < 1은 안전 인수, \alpha_{\min}과 \alpha_{\max}는 스텝 크기 변화의 하한과 상한이다.

6. 직교 제약의 수치적 유지

6.1 회전 행렬의 드리프트

실시간 제어에서 회전 행렬을 반복적으로 갱신하면 직교 제약 \mathbf{R}^T\mathbf{R} = \mathbf{I}이 점차 위반된다. k번 갱신 후의 드리프트는 대략 O(ku)이다.

재정규화 방법은 다음과 같다.

그램-슈미트 직교화: 회전 행렬의 열벡터에 그램-슈미트 과정을 적용하여 직교 정규 기저를 복원한다. 수정 그램-슈미트(Modified Gram-Schmidt) 방법이 수치적으로 더 안정적이다.

SVD 기반 사영: \mathbf{R}_{\text{drifted}} = \mathbf{U}\boldsymbol{\Sigma}\mathbf{V}^T에서 가장 가까운 직교 행렬은 \mathbf{R}_{\text{corrected}} = \mathbf{U}\mathbf{V}^T이다.

쿼터니언 정규화: 회전을 단위 쿼터니언 \mathbf{q} = (q_0, q_1, q_2, q_3)으로 표현하면, 정규화는 단순히 다음과 같다.

\hat{\mathbf{q}} = \frac{\mathbf{q}}{\|\mathbf{q}\|}

쿼터니언 표현은 정규화가 간단하고, 짐벌 록(gimbal lock)이 없으며, 보간이 용이하므로 실시간 제어에서 선호된다.

6.2 쿼터니언 기반 자세 적분

각속도 \boldsymbol{\omega}에 대한 쿼터니언의 시간 미분은 다음과 같다.

\dot{\mathbf{q}} = \frac{1}{2}\boldsymbol{\Omega}(\boldsymbol{\omega})\,\mathbf{q}

여기서 \boldsymbol{\Omega}는 다음과 같은 반대칭 행렬이다.

\boldsymbol{\Omega}(\boldsymbol{\omega}) = \begin{bmatrix} 0 & -\omega_x & -\omega_y & -\omega_z \\ \omega_x & 0 & \omega_z & -\omega_y \\ \omega_y & -\omega_z & 0 & \omega_x \\ \omega_z & \omega_y & -\omega_x & 0 \end{bmatrix}

수치 적분 후에는 \|\mathbf{q}\| = 1 제약을 유지하기 위해 매 스텝마다 정규화를 수행한다.

7. 실시간 제어 루프에서의 전략

7.1 계산 시간 관리

실시간 제어 루프는 제한된 시간(주기) 내에 모든 수치 계산을 완료해야 한다. 일반적인 로봇 제어 주기는 1~10 ms이다. 수치적 안정성과 계산 효율의 균형을 위해 다음을 고려한다.

• SVD는 정확하지만 계산 비용이 높다. 매 제어 주기마다 수행하기 어려울 수 있으므로, 조건수가 임계값을 초과할 때만 SVD를 사용하고, 그 외에는 LU 분해를 사용한다.
• 관성 행렬의 촐레스키 분해는 O(n^3/3)으로 효율적이다.
• 야코비 행렬의 해석적 계산이 가능하면 수치 미분보다 우선적으로 사용한다.

7.2 오류 검출과 안전 대응

수치적 불안정성을 실시간으로 검출하기 위한 모니터링 지표는 다음과 같다.

\begin{aligned} \text{조건수 감시}: & \quad \kappa(\mathbf{J}) > \kappa_{\text{thresh}} \\ \text{관절 속도 감시}: & \quad \|\dot{\mathbf{q}}\| > \dot{q}_{\max} \\ \text{토크 감시}: & \quad \|\boldsymbol{\tau}\| > \tau_{\max} \\ \text{위치 오차 감시}: & \quad \|\mathbf{x}_{\text{desired}} - \mathbf{x}_{\text{actual}}\| > e_{\max} \end{aligned}

이러한 조건이 감지되면 다음과 같은 안전 대응을 수행한다.

1. 감쇠 인수를 증가시켜 관절 속도를 제한한다
2. 작업 공간 속도 명령을 스케일링하여 관절 속도 한계 내로 유지한다
3. 관절 공간 제어로 전환한다
4. 비상 정지를 수행한다

7.3 수치 필터링

센서 잡음과 수치 오차의 영향을 줄이기 위해 디지털 필터를 적용한다. 저역 통과 필터(low-pass filter)의 이산 시간 구현은 다음과 같다.

y_k = \alpha\,y_{k-1} + (1 - \alpha)\,x_k

여기서 \alpha = e^{-2\pi f_c T_s}이고, f_c는 차단 주파수, T_s는 샘플링 주기이다.

수치 미분에서의 잡음 증폭을 방지하기 위해 필터링된 미분을 사용한다.

\dot{x}_k = \frac{\alpha}{T_s}(x_k - x_{k-1}) + (1 - \alpha)\,\dot{x}_{k-1}

8. 종합적 안정성 확보 프레임워크

수치적 안정성을 확보하기 위한 종합적 프레임워크를 다음과 같이 정리한다.

설계 단계:

• 적절한 좌표 표현 선택(쿼터니언 vs 오일러각 vs 회전 행렬)
• 해석적 야코비 행렬 유도
• 적절한 수치 적분 방법 선택

구현 단계:

• 배정밀도 부동 소수점 사용
• 분해 기반 선형 시스템 풀이
• 직교 제약의 주기적 재정규화

운용 단계:

• 실시간 조건수 모니터링
• 적응적 감쇠 인수 설정
• 안전 한계 감시 및 비상 대응

9. 참고 문헌

• Siciliano, B., Sciavicco, L., Villani, L., & Oriolo, G. (2009). Robotics: Modelling, Planning and Control. Springer.
• Buss, S. R., & Kim, J. S. (2005). “Selectively Damped Least Squares for Inverse Kinematics.” Journal of Graphics Tools, 10(3), 37–49.
• Featherstone, R. (2008). Rigid Body Dynamics Algorithms. Springer.
• Nakamura, Y. (1991). Advanced Robotics: Redundancy and Optimization. Addison-Wesley.
• Hairer, E., & Wanner, G. (1996). Solving Ordinary Differential Equations II: Stiff and Differential-Algebraic Problems (2nd ed.). Springer.
• Higham, N. J. (2002). Accuracy and Stability of Numerical Algorithms (2nd ed.). SIAM.
• Craig, J. J. (2005). Introduction to Robotics: Mechanics and Control (3rd ed.). Pearson Prentice Hall.

v 0.1