# 로봇의 움직임 및 조인트 제어

#### 로봇의 동역학 모델링

로봇의 동역학 모델링은 로봇의 움직임을 기술하는 수식과 모델을 정의하는 중요한 과정이다. 일반적으로, 로봇의 동역학 모델링은 뉴턴-오일러 방정식 또는 라그랑주 역학을 통해 표현할 수 있다. 라그랑주 방정식은 특히 멀티 조인트 로봇 시스템에서 주로 사용되며, 이 방정식은 다음과 같이 정의된다.

$$
\frac{d}{dt} \left( \frac{\partial L}{\partial \dot{\mathbf{q}}} \right) - \frac{\partial L}{\partial \mathbf{q}} = \mathbf{\tau}
$$

여기서:

* $\mathbf{q}$는 로봇의 일반화된 좌표 (joint positions)이다.
* $\dot{\mathbf{q}}$는 일반화된 속도 (joint velocities)이다.
* $L = T - V$는 라그랑주 함수로, $T$는 운동 에너지, $V$는 위치 에너지를 나타낸다.
* $\mathbf{\tau}$는 조인트에 적용되는 토크 또는 힘을 나타낸다.

#### 운동 에너지와 위치 에너지

로봇의 운동 에너지는 각 링크의 질량과 속도를 기반으로 계산된다. $i$번째 링크에 대한 운동 에너지는 다음과 같다:

$$
T\_i = \frac{1}{2} m\_i \dot{\mathbf{r}}\_i^\top \dot{\mathbf{r}}\_i + \frac{1}{2} \dot{\mathbf{q}}^\top \mathbf{I}\_i \dot{\mathbf{q}}
$$

여기서:

* $m\_i$는 $i$번째 링크의 질량이다.
* $\dot{\mathbf{r}}\_i$는 $i$번째 링크의 속도 벡터이다.
* $\mathbf{I}\_i$는 $i$번째 링크의 관성 모멘트 행렬이다.

위치 에너지는 링크가 위치한 높이에 따라 결정되며, 중력에 대한 포텐셜 에너지를 나타낸다. $i$번째 링크에 대한 위치 에너지는 다음과 같이 계산된다:

$$
V\_i = m\_i g h\_i
$$

여기서:

* $g$는 중력 가속도이다.
* $h\_i$는 링크의 높이이다.

#### 조인트의 종류와 동역학 방정식

조인트는 로봇의 각 링크 사이의 관계를 정의하며, 조인트에 따라 동역학 방정식이 달라진다. 조인트는 크게 회전 조인트와 직선 조인트로 나눌 수 있다. 회전 조인트는 각도와 각속도를 사용하여 정의되고, 직선 조인트는 변위와 속도를 사용하여 정의된다.

회전 조인트에 대한 동역학 방정식은 다음과 같이 표현된다:

$$
\tau\_i = I\_i \ddot{q}\_i + C\_i(\dot{q}\_i, q\_i) + G\_i(q\_i)
$$

여기서:

* $\tau\_i$는 $i$번째 조인트에 적용되는 토크이다.
* $I\_i$는 $i$번째 조인트의 관성 행렬이다.
* $C\_i(\dot{q}\_i, q\_i)$는 코리올리 및 원심력 항이다.
* $G\_i(q\_i)$는 중력에 의한 토크이다.

#### 다자유도 로봇의 동역학 모델

다자유도 로봇의 경우, 각 조인트에 대한 동역학 방정식을 결합하여 전체 로봇 시스템에 대한 동역학 모델을 구성할 수 있다. 다자유도 로봇의 동역학 모델은 다음과 같은 형태로 일반화된다:

$$
\mathbf{\tau} = \mathbf{M(q)} \ddot{\mathbf{q}} + \mathbf{C(q, \dot{q})} \dot{\mathbf{q}} + \mathbf{G(q)} + \mathbf{F\_d}
$$

여기서:

* $\mathbf{\tau}$는 각 조인트에 적용되는 토크 벡터이다.
* $\mathbf{M(q)}$는 관성 행렬 (Mass matrix)이다. 이는 로봇의 조인트 위치 $\mathbf{q}$에 따라 변한다.
* $\mathbf{C(q, \dot{q})} \dot{\mathbf{q}}$는 코리올리와 원심력 벡터이다. 이는 조인트 위치와 속도에 따라 변한다.
* $\mathbf{G(q)}$는 중력에 의한 토크 벡터이다.
* $\mathbf{F\_d}$는 마찰 및 외부 저항력이다.

**관성 행렬 $\mathbf{M(q)}$**

관성 행렬 $\mathbf{M(q)}$는 로봇의 조인트 위치에 따라 각 조인트 간의 관성 관계를 나타낸다. 다자유도 로봇에서 이 행렬은 조인트의 질량 분포와 관성을 고려하여 계산된다.

$$
\mathbf{M(q)} = \sum\_{i=1}^{n} \mathbf{J}\_i^\top \mathbf{I}\_i \mathbf{J}\_i
$$

여기서:

* $\mathbf{J}\_i$는 $i$번째 링크의 Jacobian 행렬이다.
* $\mathbf{I}\_i$는 $i$번째 링크의 관성 텐서이다.

**코리올리 및 원심력 벡터 $\mathbf{C(q, \dot{q})} \dot{\mathbf{q}}$**

코리올리 및 원심력 항은 로봇이 움직일 때 발생하는 원심력과 관성 효과를 나타낸다. 이는 주로 속도에 의존하며, 다음과 같이 계산된다:

$$
\mathbf{C(q, \dot{q})} \dot{\mathbf{q}} = \sum\_{i=1}^{n} \dot{\mathbf{q}}^\top \mathbf{C}\_i(q, \dot{q}) \dot{\mathbf{q}}
$$

**중력 벡터 $\mathbf{G(q)}$**

중력에 의한 토크 벡터는 로봇의 각 조인트에 작용하는 중력에 의한 힘을 나타낸다. 이는 각 링크의 위치 및 질량을 고려하여 계산되며, 다음과 같다:

$$
\mathbf{G(q)} = \sum\_{i=1}^{n} m\_i g \mathbf{J}\_i^\top \mathbf{r}\_i
$$

여기서:

* $m\_i$는 $i$번째 링크의 질량이다.
* $g$는 중력 가속도이다.
* $\mathbf{r}\_i$는 $i$번째 링크의 위치 벡터이다.

#### 조인트 제어 전략

로봇의 조인트를 제어하는 주요 방법은 PID 제어, 모델 기반 제어, 그리고 임피던스 제어가 있다. 이 중에서 가장 일반적으로 사용되는 방법은 PID 제어이다.

**PID 제어**

PID 제어는 비례, 적분, 미분 제어를 결합한 방식으로, 다음과 같은 제어 법칙을 따른다:

$$
u(t) = K\_p e(t) + K\_i \int e(t) dt + K\_d \frac{d e(t)}{dt}
$$

여기서:

* $u(t)$는 제어 입력이다.
* $K\_p$는 비례 제어 게인이다.
* $K\_i$는 적분 제어 게인이다.
* $K\_d$는 미분 제어 게인이다.
* $e(t) = q\_d(t) - q(t)$는 목표 조인트 위치와 실제 조인트 위치의 차이인 오차이다.

PID 제어는 각 조인트의 위치와 속도를 제어하여 원하는 동작을 수행하는 데 효과적이다. 그러나 동역학 효과를 무시하는 단점이 있어 고속 또는 고정밀 제어가 필요한 경우, 모델 기반 제어나 다른 고급 제어 기법이 필요하다.

#### 모델 기반 제어

모델 기반 제어는 로봇의 동역학 모델을 활용하여 각 조인트에 가해지는 힘과 토크를 계산하여 제어하는 방법이다. 이 방식은 PID 제어와 달리, 로봇의 관성, 코리올리 항, 중력 등을 고려한 제어가 가능하다. 모델 기반 제어의 일반적인 형태는 다음과 같다:

$$
\mathbf{\tau} = \mathbf{M(q)} \mathbf{a}\_d + \mathbf{C(q, \dot{q})} \dot{\mathbf{q}} + \mathbf{G(q)} + \mathbf{F}\_d
$$

여기서:

* $\mathbf{a}\_d$는 원하는 가속도 벡터로, 이는 제어 입력을 통해 계산된다.
* 나머지 항들은 앞서 설명한 것처럼 관성, 코리올리, 중력, 그리고 마찰력 항이다.

모델 기반 제어는 로봇의 동적 응답을 매우 정밀하게 제어할 수 있으며, 특히 고속으로 움직이거나 높은 정밀도가 요구되는 시스템에 적합하다. 이 방식은 로봇의 실제 모델과 제어기가 사용하는 모델 간의 차이에 민감하므로, 정확한 모델링이 중요하다.

**임피던스 제어**

임피던스 제어는 로봇이 환경과 상호작용할 때, 물리적인 힘과 토크를 조절하여 원하는 동작을 수행하는 제어 방법이다. 이 제어 방식은 주로 힘 제어가 중요한 경우에 사용되며, 로봇의 동작을 물리적 시스템의 임피던스 (관성, 감쇠, 강성)를 기반으로 정의한다.

임피던스 제어는 일반적으로 다음과 같은 방정식으로 정의된다:

$$
\mathbf{F}\_e = \mathbf{M}\_d (\ddot{\mathbf{x}}\_d - \ddot{\mathbf{x}}) + \mathbf{B}\_d (\dot{\mathbf{x}}\_d - \dot{\mathbf{x}}) + \mathbf{K}\_d (\mathbf{x}\_d - \mathbf{x})
$$

여기서:

* $\mathbf{F}\_e$는 외부 힘 (환경과의 상호작용에서 발생하는 힘)이다.
* $\mathbf{M}\_d$, $\mathbf{B}\_d$, $\mathbf{K}\_d$는 각각 목표 관성, 감쇠, 강성 행렬이다.
* $\mathbf{x}\_d$, $\dot{\mathbf{x}}\_d$, $\ddot{\mathbf{x}}\_d$는 각각 목표 위치, 속도, 가속도이다.
* $\mathbf{x}$, $\dot{\mathbf{x}}$, $\ddot{\mathbf{x}}$는 실제 위치, 속도, 가속도이다.

임피던스 제어는 로봇이 외부 환경에 의해 작용하는 힘을 감지하고, 그 힘에 적절하게 대응할 수 있도록 제어하는 데 효과적이다. 이를 통해 로봇은 부드럽게 환경과 상호작용할 수 있으며, 주로 협동 로봇이나 의료용 로봇 등에서 사용된다.

#### 역운동학을 통한 제어

로봇의 움직임을 제어하기 위해서는 역운동학이 중요한 역할을 한다. 역운동학은 로봇의 끝단(End-Effector)의 목표 위치를 기반으로 각 조인트의 위치를 계산하는 방법이다. 주어진 목표 위치 $\mathbf{x}\_d$에 대해, 이를 만족하는 조인트의 위치 $\mathbf{q}$를 찾아내는 과정이 역운동학이다.

역운동학 방정식은 다음과 같이 표현할 수 있다:

$$
\mathbf{x}\_d = f(\mathbf{q})
$$

여기서:

* $\mathbf{x}\_d$는 로봇 끝단의 목표 위치 벡터이다.
* $\mathbf{q}$는 조인트 변수 벡터이다.
* $f(\mathbf{q})$는 정운동학 방정식으로, 조인트 변수로부터 로봇 끝단의 위치를 계산하는 함수이다.

역운동학의 목표는 주어진 $\mathbf{x}\_d$에 대해 이를 만족하는 $\mathbf{q}$를 구하는 것이며, 이는 비선형 방정식을 푸는 문제로 귀결된다. 다자유도 로봇에서는 여러 해가 존재할 수 있으며, 이는 로봇의 조인트 구성에 따라 다르다.

#### 역운동학의 수치적 해법

역운동학 방정식은 비선형이므로 일반적으로 수치적인 방법을 사용하여 해를 구한다. 가장 널리 사용되는 방법 중 하나는 뉴턴-랩슨 방법 또는 Jacobian 행렬을 이용한 반복적 방법이다. 이 방법은 목표 위치 $\mathbf{x}\_d$와 현재 위치 $\mathbf{x}$ 간의 오차를 줄여나가는 방식으로, Jacobian 행렬을 활용한다.

Jacobian 행렬 $\mathbf{J(q)}$는 다음과 같이 정의된다:

$$
\mathbf{J(q)} = \frac{\partial f(\mathbf{q})}{\partial \mathbf{q}}
$$

여기서:

* $\mathbf{J(q)}$는 로봇의 각 조인트 변수에 대한 로봇 끝단 위치의 변화율을 나타내는 행렬이다.
* $f(\mathbf{q})$는 정운동학 방정식이다.

목표 위치와 실제 위치 사이의 오차 $\Delta \mathbf{x} = \mathbf{x}\_d - \mathbf{x}$를 줄이기 위해, 다음과 같은 방식으로 조인트 변수를 업데이트한다:

$$
\Delta \mathbf{q} = \mathbf{J(q)}^{-1} \Delta \mathbf{x}
$$

따라서, 조인트 변수는 다음과 같이 갱신된다:

$$
\mathbf{q}*{\text{new}} = \mathbf{q}*{\text{old}} + \Delta \mathbf{q}
$$

이 과정을 반복하여 오차 $\Delta \mathbf{x}$가 충분히 작아질 때까지 조인트 변수를 수정해 나간다. 이 방법은 Jacobian 행렬이 정방행렬이거나, 최소 제곱 해법을 이용해 역행렬을 계산할 수 있을 때 유효하다.

#### Jacobian Transpose 방법

Jacobian 행렬의 역행렬을 구하기 어려운 경우, Jacobian의 전치 행렬을 이용한 방법도 있다. 이는 특히 Jacobian이 정방 행렬이 아닐 때 유용하다. 이 방법에서는 다음과 같은 업데이트 법칙을 사용한다:

$$
\Delta \mathbf{q} = \alpha \mathbf{J(q)}^\top \Delta \mathbf{x}
$$

여기서:

* $\alpha$는 적절한 스칼라 계수로, 수렴 속도를 조절한다.
* $\mathbf{J(q)}^\top$는 Jacobian의 전치 행렬이다.

Jacobian Transpose 방법은 계산이 간단하고 수렴성이 좋다는 장점이 있다. 그러나 이 방법은 Jacobian의 상태에 따라 정확도가 떨어질 수 있으므로, $\alpha$의 값을 적절히 설정하는 것이 중요하다.

#### Jacobian Pseudo-Inverse 방법

Jacobian이 정방 행렬이 아니거나 Jacobian이 특이한 경우, Jacobian의 유사역행렬 (Pseudo-Inverse)을 사용할 수 있다. 이는 최소 제곱 해법을 적용하여, 목표 위치와 실제 위치 사이의 오차를 최소화하는 방향으로 조인트 변수를 갱신하는 방법이다.

유사역행렬을 이용한 조인트 변수 갱신은 다음과 같이 표현된다:

$$
\Delta \mathbf{q} = \mathbf{J(q)}^\dagger \Delta \mathbf{x}
$$

여기서:

* $\mathbf{J(q)}^\dagger$는 Jacobian의 유사역행렬이다.

유사역행렬은 Moore-Penrose 유사역행렬로 정의되며, 다음과 같이 구할 수 있다:

$$
\mathbf{J(q)}^\dagger = (\mathbf{J(q)}^\top \mathbf{J(q)})^{-1} \mathbf{J(q)}^\top
$$

이 방법은 조인트 변수의 수가 로봇의 자유도보다 많을 때나, Jacobian이 특이점을 가질 때 유용하다.

#### 조인트 제어 시 고려 사항

로봇의 움직임을 제어할 때, 조인트의 물리적 한계, 속도 제한, 토크 제한 등을 고려해야 한다. 이러한 물리적 한계를 무시하면 로봇에 손상을 입힐 수 있다. 따라서 제어 알고리즘을 설계할 때 이러한 제한을 반영하는 것이 중요하다.

또한, 제어 알고리즘은 로봇이 고속으로 움직일 때 발생하는 동역학적 효과를 적절히 처리해야 하며, 이를 위해 모델 기반 제어나 고급 제어 기법을 사용할 수 있다.

#### 코드 예시

조인트 제어를 위한 간단한 PID 제어 코드는 다음과 같다:

```cpp
// 원하는 조인트 위치
double q_desired = 1.0; 

// 실제 조인트 위치와 속도
double q = getJointPosition();
double q_dot = getJointVelocity();

// PID 제어 입력 계산
double error = q_desired - q;
double control_input = Kp * error + Kd * (q_dot - error);

// 조인트에 제어 입력 적용
setJointTorque(control_input);
```

이 코드는 간단한 PID 제어로 조인트 위치를 제어하는 방법을 보여준다. 이때 $K\_p$와 $K\_d$는 제어 게인으로, 로봇의 응답에 맞게 조정해야 한다.