로봇 공학에서의 활용

확장 칼만 필터의 로봇 공학적 적용

확장 칼만 필터(EKF)는 로봇 공학에서 주로 이동 로봇의 위치 추정 및 경로 계획을 위해 사용된다. 로봇 공학의 여러 분야에서 비선형 시스템이 흔히 존재하며, 이러한 비선형성을 처리하기 위해 EKF가 필수적이다. 로봇 공학에서 EKF의 주요 활용 분야는 로봇의 위치 추정, 경로 추정, 그리고 센서 융합을 통한 정밀한 상태 추정이다.

로봇의 상태 추정

로봇의 상태는 일반적으로 위치, 속도, 가속도, 그리고 각도를 포함한 다양한 물리적 변수로 정의된다. 예를 들어, 2D 평면에서 움직이는 로봇의 상태는 다음과 같은 벡터로 표현될 수 있다:

$\mathbf{x} = \begin{bmatrix} x \\ y \\ \theta \end{bmatrix}$

여기서 $x$ 와 $y$ 는 로봇의 좌표, $\theta$ 는 로봇의 방향을 의미한다.

확장 칼만 필터는 이러한 상태를 추정하기 위해 다음과 같은 상태 공간 모델을 사용한다:

$\mathbf{x}_{k+1} = f(\mathbf{x}_k, \mathbf{u}_k) + \mathbf{w}_k$

여기서: - $\mathbf{x}_k$ 는 $k$ 번째 상태 벡터 - $\mathbf{u}_k$ 는 입력 제어 벡터 (예: 로봇의 속도 및 회전 속도) - $f(\mathbf{x}_k, \mathbf{u}_k)$ 는 비선형 상태 전이 함수 - $\mathbf{w}_k$ 는 시스템 노이즈 벡터로, 잡음 공분산 $\mathbf{Q}_k$ 를 따른다.

비선형 관측 모델

로봇의 상태는 다양한 센서들로부터 관측되며, 이러한 센서 데이터는 종종 비선형적인 방식으로 로봇의 상태에 종속된다. 예를 들어, 로봇이 LiDAR나 카메라를 사용하여 주변 환경을 인식할 때, 그 관측 값은 로봇의 상태와 비선형적으로 연결된다. 이러한 관측 모델은 다음과 같이 정의된다:

$\mathbf{z}_k = h(\mathbf{x}_k) + \mathbf{v}_k$

여기서: - $\mathbf{z}_k$ 는 센서로부터의 관측 벡터 - $h(\mathbf{x}_k)$ 는 상태와 센서 관측을 연결하는 비선형 함수 - $\mathbf{v}_k$ 는 관측 잡음 벡터로, 잡음 공분산 $\mathbf{R}_k$ 를 따른다.

선형화 과정

EKF의 핵심은 비선형 시스템을 선형화하여 칼만 필터의 이론을 적용하는 데 있다. 이를 위해 테일러 급수를 이용한 선형화가 필요하다. 상태 전이 함수 $f(\mathbf{x}_k, \mathbf{u}_k)$ 와 관측 함수 $h(\mathbf{x}_k)$ 는 각각 야코비 행렬을 통해 선형화된다.

상태 전이 함수의 야코비 행렬 $\mathbf{F}_k$ 는 다음과 같이 정의된다:

$\mathbf{F}_k = \frac{\partial f}{\partial \mathbf{x}} \bigg|_{\mathbf{x} = \hat{\mathbf{x}}_k}$

관측 함수의 야코비 행렬 $\mathbf{H}_k$ 는 다음과 같다:

$\mathbf{H}_k = \frac{\partial h}{\partial \mathbf{x}} \bigg|_{\mathbf{x} = \hat{\mathbf{x}}_k}$

이러한 야코비 행렬을 이용해 확장 칼만 필터는 다음과 같은 단계로 상태를 추정한다.

예측 단계

예측 단계에서는 이전 상태를 바탕으로 새로운 상태를 예측한다. 예측된 상태는 다음과 같이 계산된다:

$\hat{\mathbf{x}}_{k+1|k} = f(\hat{\mathbf{x}}_{k|k}, \mathbf{u}_k)$

예측 오차 공분산은 다음과 같다:

$\mathbf{P}_{k+1|k} = \mathbf{F}_k \mathbf{P}_{k|k} \mathbf{F}_k^\top + \mathbf{Q}_k$

여기서 $\mathbf{P}_{k|k}$ 는 이전 상태의 오차 공분산 행렬이다.

업데이트 단계

업데이트 단계에서는 센서로부터 얻은 관측 데이터를 이용하여 예측된 상태를 보정한다. 이 단계는 다음의 과정을 따른다.

잔차 계산 (Innovation)
관측 값과 예측 값 사이의 차이를 잔차(Innovation)라고 한다. 잔차는 다음과 같이 정의된다:

$\mathbf{y}_k = \mathbf{z}_k - h(\hat{\mathbf{x}}_{k|k-1})$

여기서 $\mathbf{z}_k$ 는 실제 관측 값, $h(\hat{\mathbf{x}}_{k|k-1})$ 는 예측된 관측 값이다.

잔차 공분산
잔차의 공분산은 관측 오차와 예측 오차를 포함한 공분산 행렬로 계산된다:

$\mathbf{S}_k = \mathbf{H}_k \mathbf{P}_{k|k-1} \mathbf{H}_k^\top + \mathbf{R}_k$

여기서 $\mathbf{R}_k$ 는 센서 잡음의 공분산 행렬이다.

칼만 이득 (Kalman Gain)
칼만 이득은 예측된 상태를 얼마나 보정할지 결정하는 값으로, 다음과 같이 계산된다:

$\mathbf{K}_k = \mathbf{P}_{k|k-1} \mathbf{H}_k^\top \mathbf{S}_k^{-1}$

칼만 이득은 잔차의 중요도를 반영하며, 값이 클수록 관측 값에 대한 신뢰가 높다는 것을 의미한다.

상태 보정
상태 벡터는 칼만 이득을 이용해 예측된 상태를 보정한다:

$\hat{\mathbf{x}}_{k|k} = \hat{\mathbf{x}}_{k|k-1} + \mathbf{K}_k \mathbf{y}_k$

오차 공분산 행렬 보정
보정된 상태에 따라 오차 공분산 행렬도 업데이트된다:

$\mathbf{P}_{k|k} = (\mathbf{I} - \mathbf{K}_k \mathbf{H}_k) \mathbf{P}_{k|k-1}$

여기서 $\mathbf{I}$ 는 항등 행렬이다.

로봇 공학의 다양한 적용 예

확장 칼만 필터는 로봇 공학에서 위치 추정, 속도 추정, 각도 추정 등 다양한 문제에 적용된다. 로봇 공학에서 센서 데이터를 융합하여 보다 정확한 상태 추정을 할 때 EKF가 자주 사용된다. 특히 로봇이 이동할 때 다양한 센서로부터 얻는 데이터는 종종 비선형적인 관계를 가지기 때문에, EKF를 통해 이러한 데이터를 효과적으로 처리할 수 있다.

자율 주행 로봇에서의 위치 추정
자율 주행 로봇은 주로 LiDAR, IMU, GPS 등의 센서를 이용하여 로봇의 현재 위치를 추정한다. EKF는 이러한 센서 데이터를 통합하여 로봇의 정확한 위치를 추정하는 데 사용된다. 예를 들어, IMU는 높은 주파수로 데이터를 제공하지만 드리프트가 발생할 수 있으며, GPS는 느리지만 절대 위치를 제공할 수 있다. EKF는 이러한 센서의 장점을 결합하여 보다 정확한 위치 추정을 가능하게 한다.
모바일 로봇의 경로 추정
모바일 로봇이 주행 경로를 추정할 때, EKF는 로봇의 동적 모델을 이용하여 미래의 상태를 예측하고, 센서로부터 얻은 데이터를 통합하여 경로를 추정한다. 이때, 로봇의 속도와 회전 속도를 입력으로 사용하여 로봇의 새로운 위치와 방향을 계산하고, 센서 데이터를 이용해 그 값을 보정한다.
로봇 매니퓰레이터의 자세 추정
로봇 매니퓰레이터는 일반적으로 다수의 조인트를 가지며, 조인트 각도를 정확하게 추정하는 것이 중요하다. 이때 EKF는 각 조인트의 각도와 속도를 추정하는 데 사용된다. 조인트에 부착된 엔코더와 IMU로부터 얻은 데이터를 이용해 매니퓰레이터의 자세를 추정하고, 동적 모델을 통해 다음 상태를 예측할 수 있다.
다중 센서 융합
로봇은 여러 센서의 데이터를 결합하여 정확한 상태 추정을 할 수 있다. EKF는 다양한 센서에서 비선형 데이터를 처리할 수 있는 능력을 제공하며, 로봇의 동적 상태를 추정하는 데 필수적인 도구로 사용된다.