6.110 자기 운동과 장애물 회피 응용

1. 자기 운동의 정의와 특성

자기 운동(self-motion)은 여유 자유도 로봇에서 말단 장치의 위치와 자세를 고정한 상태로 관절 형상만을 변화시키는 운동이다. 자코비안의 영 공간에 속하는 관절 속도에 의해 발생하며, 다음 조건을 만족한다.

$J(\mathbf{q})\dot{\mathbf{q}}_{\text{self}} = \mathbf{0}, \quad \dot{\mathbf{q}}_{\text{self}} \in \mathcal{N}(J)$

자기 운동은 관절 공간에서 자기 운동 매니폴드(self-motion manifold) 위를 이동하는 궤적으로 해석된다. 순기구학 함수 $f: \mathbb{R}^n \to \mathbb{R}^m$ 에 대하여, 말단 장치가 고정된 작업 공간 위치 $\mathbf{x}_0$ 에 대응하는 자기 운동 매니폴드는 다음과 같다.

$\mathcal{M}_{\mathbf{x}_0} = f^{-1}(\mathbf{x}_0) = \{\mathbf{q} \in \mathbb{R}^n \mid f(\mathbf{q}) = \mathbf{x}_0\}$

비특이 형상에서 이 매니폴드의 차원은 $n - m$ 이며, 각 점 $\mathbf{q}$ 에서의 접선 공간이 $\mathcal{N}(J(\mathbf{q}))$ 이다.

2. 자기 운동의 매개변수화

2.1 자유도 로봇 팔의 팔꿈치 각도

7자유도 로봇 팔( $n = 7$ , $m = 6$ )에서 자기 운동 매니폴드는 1차원이며, 팔꿈치 순환 각도(swivel angle) $\phi$ 로 매개변수화할 수 있다. 어깨와 손목의 위치가 고정된 상태에서, 팔꿈치는 어깨-손목 축을 중심으로 원을 그리며 이동한다.

팔꿈치 위치 $\mathbf{p}_e(\phi)$ 는 다음과 같이 표현된다.

$\mathbf{p}_e(\phi) = \mathbf{p}_c + r(\cos\phi \, \hat{\mathbf{u}} + \sin\phi \, \hat{\mathbf{v}})$

여기서 $\mathbf{p}_c$ 는 순환 원의 중심, $r$ 은 반지름, $\hat{\mathbf{u}}$ 와 $\hat{\mathbf{v}}$ 는 순환 평면의 정규 직교 기저 벡터이다.

일반적 매개변수화

$n - m > 1$ 인 경우, 자기 운동 매니폴드는 다차원이며 여러 매개변수 $\phi_1, \phi_2, \ldots, \phi_{n-m}$ 으로 기술된다. 이 매개변수의 선택은 일반적으로 유일하지 않으며, 작업의 특성에 따라 물리적으로 의미 있는 매개변수를 선정하는 것이 바람직하다.

장애물 회피 문제의 정식화

로봇이 작업 공간에서 장애물과 충돌하지 않으면서 주 작업을 수행해야 할 때, 장애물 회피를 부차 목표로 설정하고 자기 운동을 통해 이를 실현할 수 있다.

거리 함수의 정의

로봇 링크 위의 제어점(control point) $\mathbf{p}_i(\mathbf{q})$ 와 장애물 표면 위의 최근접점 $\mathbf{o}_i$ 사이의 거리를 다음과 같이 정의한다.

$d_i(\mathbf{q}) = \|\mathbf{p}_i(\mathbf{q}) - \mathbf{o}_i\|$

로봇 전체와 장애물 사이의 최소 거리는 다음과 같다.

$d_{\min}(\mathbf{q}) = \min_i d_i(\mathbf{q})$

장애물 회피 목적 함수

장애물 회피를 위한 대표적인 목적 함수는 다음과 같다.

방법 1: 최소 거리 최대화

$w_{\text{obs}}(\mathbf{q}) = d_{\min}(\mathbf{q})$

이 함수를 최대화하면 로봇과 장애물 사이의 최소 거리가 증가한다.

방법 2: 인공 퍼텐셜 함수

$w_{\text{rep}}(\mathbf{q}) = \begin{cases} -\frac{1}{2}\eta\left(\frac{1}{d_{\min}(\mathbf{q})} - \frac{1}{d_0}\right)^2 & \text{if } d_{\min}(\mathbf{q}) \leq d_0 \\ 0 & \text{if } d_{\min}(\mathbf{q}) > d_0 \end{cases}$

여기서 $\eta > 0$ 은 반발력 이득이고, $d_0 > 0$ 은 장애물의 영향 범위를 결정하는 임계 거리이다. 이 함수를 최대화(즉, 절대값을 최소화)하면 장애물 근처에서 반발력이 생성된다.

방법 3: 로그 장벽 함수

$w_{\text{log}}(\mathbf{q}) = \sum_i \ln(d_i(\mathbf{q}))$

이 함수는 거리가 0에 접근할 때 $-\infty$ 로 발산하므로, 강한 장벽 효과를 제공한다.

3. 제어점 자코비안

제어점 $\mathbf{p}_i(\mathbf{q})$ 의 관절 각도에 대한 자코비안은 다음과 같이 정의된다.

$J_{p_i}(\mathbf{q}) = \frac{\partial \mathbf{p}_i}{\partial \mathbf{q}} \in \mathbb{R}^{3 \times n}$

거리 함수의 기울기는 제어점 자코비안을 이용하여 다음과 같이 계산된다.

$\nabla d_i = \frac{\partial d_i}{\partial \mathbf{q}} = \frac{(\mathbf{p}_i - \mathbf{o}_i)^\top}{d_i} J_{p_i} \in \mathbb{R}^{1 \times n}$

따라서 기울기 벡터는 다음과 같다.

$(\nabla d_i)^\top = J_{p_i}^\top \frac{\mathbf{p}_i - \mathbf{o}_i}{d_i} = J_{p_i}^\top \hat{\mathbf{n}}_i$

여기서 $\hat{\mathbf{n}}_i = \frac{\mathbf{p}_i - \mathbf{o}_i}{\|\mathbf{p}_i - \mathbf{o}_i\|}$ 는 장애물로부터 제어점을 향하는 단위 벡터이다.

영 공간 투영을 이용한 장애물 회피

기울기 투영법

주 작업을 수행하면서 장애물을 회피하는 관절 속도는 다음과 같다.

$\dot{\mathbf{q}} = J^+ \dot{\mathbf{x}} + \alpha (I - J^+ J) (\nabla d_{\min})^\top$

이 방법은 최소 거리에 해당하는 제어점에 대해서만 반발력을 생성하므로, 다수의 장애물이 존재하는 환경에서 제어점 전환 시 불연속이 발생할 수 있다.

3.1 다중 제어점 방법

여러 제어점에 대해 동시에 장애물 회피를 수행하려면, 각 제어점의 기여를 합산한다.

$\mathbf{z}_{\text{obs}} = \sum_{i} w_i \, J_{p_i}^\top \hat{\mathbf{n}}_i$

여기서 $w_i$ 는 거리에 따른 가중치로, 장애물에 가까운 제어점일수록 큰 값을 부여한다.

$w_i = \begin{cases} \frac{1}{d_i^2}\left(\frac{1}{d_i} - \frac{1}{d_0}\right) & \text{if } d_i \leq d_0 \\ 0 & \text{if } d_i > d_0 \end{cases}$

전체 관절 속도는 다음과 같다.

$\dot{\mathbf{q}} = J^+ \dot{\mathbf{x}} + \alpha (I - J^+ J) \mathbf{z}_{\text{obs}}$

작업 우선순위 기반 방법

장애물 회피를 주 작업보다 높은 우선순위로 설정할 수도 있다. 이 경우, 장애물 회피 작업을 제1 우선순위로, 말단 장치 속도 추종을 제2 우선순위로 설정한다.

$\dot{\mathbf{q}} = J_{\text{obs}}^+ \dot{\mathbf{x}}_{\text{obs}} + (I - J_{\text{obs}}^+ J_{\text{obs}}) J^{+\prime} \dot{\mathbf{x}}$

여기서 $J_{\text{obs}} = \hat{\mathbf{n}}_i^\top J_{p_i}$ 는 장애물 회피 작업의 자코비안이고, $\dot{\mathbf{x}}_{\text{obs}}$ 는 장애물로부터의 원하는 이탈 속도이다. $J^{+\prime} = (J(I - J_{\text{obs}}^+ J_{\text{obs}}))^+$ 는 장애물 회피 작업의 영 공간에 제한된 유사 역행렬이다.

이 방법은 장애물 충돌 위험이 높을 때 주 작업의 정확도를 일부 희생하더라도 안전을 보장하는 장점이 있다.

4. 활성화 함수를 이용한 점진적 전환

장애물 회피의 활성화를 점진적으로 전환하기 위해 활성화 함수(activation function)를 도입한다.

$h(d) = \begin{cases} 1 & \text{if } d \leq d_1 \\ \frac{d_0 - d}{d_0 - d_1} & \text{if } d_1 < d < d_0 \\ 0 & \text{if } d \geq d_0 \end{cases}$

여기서 $d_1 < d_0$ 은 완전 활성화 임계 거리이고, $d_0$ 은 비활성화 임계 거리이다. 활성화 함수를 적용한 관절 속도는 다음과 같다.

$\dot{\mathbf{q}} = J^+ \dot{\mathbf{x}} + \alpha \, h(d_{\min}) \, N \, \mathbf{z}_{\text{obs}}$

이 방법을 통해 장애물이 멀리 있을 때 불필요한 내부 운동을 억제하고, 장애물에 접근할 때 자연스럽게 회피 운동을 생성할 수 있다.

5. 자기 운동의 한계와 대응

5.1 자기 운동 매니폴드의 유한성

자기 운동 매니폴드가 유한한 범위를 가지는 경우, 영 공간 투영만으로는 충분한 장애물 회피 거리를 확보하지 못할 수 있다. 이는 관절 한계의 존재와 기구학적 구조에 의한 제약으로 발생한다.

5.2 특이점 근방의 불안정성

자코비안의 랭크가 감소하는 특이 형상 근방에서 영 공간의 차원이 증가하며, 영 공간 투영 행렬이 급격히 변화한다. 감쇠 최소 제곱법을 적용하여 이를 안정화할 수 있다.

$J_\lambda^+ = J^\top (JJ^\top + \lambda^2 I)^{-1}$

국소 최솟값 문제

인공 퍼텐셜 함수 기반 방법은 국소 최솟값(local minimum)에 빠질 수 있다. 이를 해결하기 위해 무작위 섭동, 탐색 기반 계획, 또는 전역 경로 계획과의 결합이 필요하다.

실시간 구현 알고리즘

알고리즘 6.110.1: 자기 운동 기반 장애물 회피

현재 관절 형상 $\mathbf{q}$ 에서 순기구학을 계산한다.
각 링크의 제어점 위치 $\mathbf{p}_i(\mathbf{q})$ 를 계산한다.
각 제어점에 대해 장애물까지의 최소 거리 $d_i$ 와 최근접점 $\mathbf{o}_i$ 를 구한다.
자코비안 $J(\mathbf{q})$ 와 제어점 자코비안 $J_{p_i}(\mathbf{q})$ 를 계산한다.
유사 역행렬 $J^+$ 와 영 공간 투영 행렬 $N = I - J^+J$ 를 계산한다.
장애물 회피 벡터 $\mathbf{z}_{\text{obs}}$ 를 계산한다.
관절 속도를 결정한다: $\dot{\mathbf{q}} = J^+ \dot{\mathbf{x}} + \alpha \, h(d_{\min}) \, N \, \mathbf{z}_{\text{obs}}$ .
관절 각도를 갱신한다: $\mathbf{q} \leftarrow \mathbf{q} + \dot{\mathbf{q}} \Delta t$ .

수치 예제

3자유도 평면 로봇( $n = 3$ , $m = 2$ )에서 두 번째 링크의 중점이 장애물에 가장 가까운 제어점이라 하자. 이 제어점의 자코비안이 다음과 같다고 가정한다.

$J_{p_2} = \begin{bmatrix} -0.8 & -0.3 & 0 \\ 0.6 & 0.4 & 0 \end{bmatrix}$

장애물로부터 제어점을 향하는 단위 벡터가 $\hat{\mathbf{n}} = (0.6, 0.8)^\top$ 이면, 장애물 회피 기울기 벡터는 다음과 같다.

$\mathbf{z}_{\text{obs}} = J_{p_2}^\top \hat{\mathbf{n}} = \begin{bmatrix} -0.8 & 0.6 \\ -0.3 & 0.4 \\ 0 & 0 \end{bmatrix} \begin{bmatrix} 0.6 \\ 0.8 \end{bmatrix} = \begin{bmatrix} 0 \\ 0.14 \\ 0 \end{bmatrix}$

이 벡터를 영 공간에 투영하면 말단 장치를 움직이지 않으면서 두 번째 링크를 장애물로부터 멀어지게 하는 관절 속도를 얻게 된다.

참고문헌

Siciliano, B., Sciavicco, L., Villani, L., & Oriolo, G. (2009). Robotics: Modelling, Planning and Control. Springer.
Nakamura, Y. (1991). Advanced Robotics: Redundancy and Optimization. Addison-Wesley.
Khatib, O. (1986). Real-time obstacle avoidance for manipulators and mobile robots. The International Journal of Robotics Research, 5(1), 90–98.
Maciejewski, A. A., & Klein, C. A. (1985). Obstacle avoidance for kinematically redundant manipulators in dynamically varying environments. The International Journal of Robotics Research, 4(3), 109–117.

v 0.1