모바일 로봇 위치 추정 - 소프트웨어 융합

모바일 로봇의 비선형 모델

모바일 로봇의 위치 추정은 비선형 시스템을 기반으로 한다. 확장 칼만 필터는 이러한 비선형성을 다루기 위해 사용된다. 모바일 로봇의 상태 벡터는 보통 위치와 속도로 표현되며, 이는 시간에 따라 변화하는 동적 시스템으로 정의할 수 있다. 일반적인 모바일 로봇의 상태 벡터 $\mathbf{x}(t)$ 는 다음과 같다:

$\mathbf{x}(t) = \begin{bmatrix} x(t) \\ y(t) \\ \theta(t) \\ v(t) \end{bmatrix}$

여기서, - $x(t)$ 는 시간 $t$ 에서의 $x$ 좌표, - $y(t)$ 는 시간 $t$ 에서의 $y$ 좌표, - $\theta(t)$ 는 로봇의 방향 각도 (Yaw), - $v(t)$ 는 시간 $t$ 에서의 선형 속도이다.

모바일 로봇의 비선형 시스템은 다음과 같은 운동 방정식으로 기술할 수 있다:

$\mathbf{\dot{x}}(t) = \begin{bmatrix} v(t) \cos(\theta(t)) \\ v(t) \sin(\theta(t)) \\ \omega(t) \\ a(t) \end{bmatrix}$

여기서, - $\omega(t)$ 는 시간 $t$ 에서의 각속도, - $a(t)$ 는 시간 $t$ 에서의 선형 가속도이다.

확장 칼만 필터 적용

모바일 로봇의 비선형 시스템을 선형화하기 위해, 확장 칼만 필터는 상태 예측을 테일러 급수를 통해 1차 근사로 나타낸다. 이는 야코비 행렬을 사용하여 선형화를 수행하는 과정이다. 예측 단계에서 사용할 시스템 모델의 선형화는 다음과 같다:

$\mathbf{F}(t) = \frac{\partial \mathbf{f}(\mathbf{x}(t))}{\partial \mathbf{x}(t)}$

이때 $\mathbf{F}(t)$ 는 상태 벡터의 변화율에 대한 야코비 행렬로, 시스템의 동적 특성을 선형 근사한 것이다.

모바일 로봇의 시스템 모델에서 야코비 행렬 $\mathbf{F}(t)$ 은 다음과 같은 형태를 가진다:

$\mathbf{F}(t) = \begin{bmatrix} 1 & 0 & -v(t) \sin(\theta(t)) & \cos(\theta(t)) \\ 0 & 1 & v(t) \cos(\theta(t)) & \sin(\theta(t)) \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix}$

이 야코비 행렬은 확장 칼만 필터의 예측 단계에서 상태 추정을 위한 필수 요소이다. 이를 바탕으로 오차 공분산 행렬 $\mathbf{P}(t)$ 을 업데이트할 수 있다.

상태 예측 및 업데이트

확장 칼만 필터의 예측 단계는 다음과 같이 정의된다:

$\mathbf{\hat{x}}(t|t-1) = \mathbf{f}(\mathbf{\hat{x}}(t-1|t-1))$

여기서 $\mathbf{\hat{x}}(t|t-1)$ 는 $t-1$ 시점에서의 정보를 바탕으로 예측된 $t$ 시점의 상태 벡터이다. 예측된 오차 공분산 행렬은 다음과 같이 계산된다:

$\mathbf{P}(t|t-1) = \mathbf{F}(t) \mathbf{P}(t-1|t-1) \mathbf{F}^T(t) + \mathbf{Q}(t)$

여기서 $\mathbf{Q}(t)$ 는 시스템의 잡음 공분산 행렬이다.

측정 모델

모바일 로봇에서 측정되는 값은 일반적으로 위치 정보( $x$ , $y$ )와 방향 각도 $\theta$ 이다. 이를 측정 모델로 표현하면 다음과 같다:

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

여기서, - $\mathbf{z}(t)$ 는 $t$ 시점에서의 측정 벡터, - $\mathbf{h}(\mathbf{x}(t))$ 는 상태 벡터와 관련된 측정 함수, - $\mathbf{v}(t)$ 는 측정 잡음 벡터이다.

측정 함수 $\mathbf{h}(\mathbf{x}(t))$ 의 선형화는 야코비 행렬 $\mathbf{H}(t)$ 로 나타낼 수 있다:

$\mathbf{H}(t) = \frac{\partial \mathbf{h}(\mathbf{x}(t))}{\partial \mathbf{x}(t)}$

모바일 로봇의 경우, 측정 함수는 다음과 같이 정의할 수 있다:

$\mathbf{h}(\mathbf{x}(t)) = \begin{bmatrix} x(t) \\ y(t) \\ \theta(t) \end{bmatrix}$

따라서 야코비 행렬 $\mathbf{H}(t)$ 은 다음과 같다:

$\mathbf{H}(t) = \begin{bmatrix} 1 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & 0 \end{bmatrix}$

측정 업데이트

측정 단계에서 확장 칼만 필터는 다음과 같이 상태 벡터를 업데이트한다:

$\mathbf{K}(t) = \mathbf{P}(t|t-1) \mathbf{H}^T(t) \left( \mathbf{H}(t) \mathbf{P}(t|t-1) \mathbf{H}^T(t) + \mathbf{R}(t) \right)^{-1}$

여기서 $\mathbf{K}(t)$ 는 칼만 이득(Kalman gain)으로, 측정과 예측의 불확실성을 고려하여 상태 벡터를 업데이트하는 역할을 한다. $\mathbf{R}(t)$ 는 측정 잡음의 공분산 행렬이다.

상태 벡터는 다음과 같이 업데이트된다:

$\mathbf{\hat{x}}(t|t) = \mathbf{\hat{x}}(t|t-1) + \mathbf{K}(t) \left( \mathbf{z}(t) - \mathbf{h}(\mathbf{\hat{x}}(t|t-1)) \right)$

오차 공분산 행렬은 다음과 같이 업데이트된다:

$\mathbf{P}(t|t) = \left( \mathbf{I} - \mathbf{K}(t) \mathbf{H}(t) \right) \mathbf{P}(t|t-1)$

C++ Eigen 구현

아래는 확장 칼만 필터를 모바일 로봇 위치 추정에 적용한 C++ 코드이다. 이 코드는 Eigen 라이브러리를 사용하여 상태 예측, 오차 공분산 예측, 측정 업데이트 등을 수행한다.

#include <iostream>
#include <Eigen/Dense>

using namespace Eigen;

// 상태 벡터의 차원 (x, y, theta, v)
const int STATE_DIM = 4;

// 측정 벡터의 차원 (x, y, theta)
const int MEASURE_DIM = 3;

// 시스템 노이즈 공분산 행렬 (임의의 값, 필요에 따라 수정)
MatrixXd processNoiseCovariance(STATE_DIM, STATE_DIM);
MatrixXd measurementNoiseCovariance(MEASURE_DIM, MEASURE_DIM);

// 함수: 상태 예측
VectorXd predictState(const VectorXd& x_prev, double dt) {
    VectorXd x_pred(STATE_DIM);
    double theta = x_prev(2);
    double v = x_prev(3);

    // 상태 예측 방정식
    x_pred(0) = x_prev(0) + v * cos(theta) * dt;
    x_pred(1) = x_prev(1) + v * sin(theta) * dt;
    x_pred(2) = theta; // 방향각 유지 (추정 모델에 따라 다를 수 있음)
    x_pred(3) = v;     // 속도 유지

    return x_pred;
}

// 함수: 오차 공분산 예측
MatrixXd predictCovariance(const MatrixXd& P_prev, const VectorXd& x_prev, double dt) {
    MatrixXd F(STATE_DIM, STATE_DIM);
    double theta = x_prev(2);
    double v = x_prev(3);

    // 야코비 행렬 F 계산
    F << 1, 0, -v * sin(theta) * dt, cos(theta) * dt,
         0, 1,  v * cos(theta) * dt, sin(theta) * dt,
         0, 0, 1, 0,
         0, 0, 0, 1;

    // 오차 공분산 예측
    MatrixXd P_pred = F * P_prev * F.transpose() + processNoiseCovariance;
    return P_pred;
}

// 함수: 칼만 이득 계산
MatrixXd computeKalmanGain(const MatrixXd& P_pred, const MatrixXd& H) {
    MatrixXd S = H * P_pred * H.transpose() + measurementNoiseCovariance;
    MatrixXd K = P_pred * H.transpose() * S.inverse();
    return K;
}

// 함수: 상태 업데이트
VectorXd updateState(const VectorXd& x_pred, const VectorXd& z, const MatrixXd& K, const MatrixXd& H) {
    VectorXd y = z - H * x_pred; // 측정 오차
    VectorXd x_updated = x_pred + K * y; // 상태 업데이트
    return x_updated;
}

// 함수: 오차 공분산 업데이트
MatrixXd updateCovariance(const MatrixXd& P_pred, const MatrixXd& K, const MatrixXd& H) {
    MatrixXd I = MatrixXd::Identity(STATE_DIM, STATE_DIM);
    MatrixXd P_updated = (I - K * H) * P_pred; // 오차 공분산 업데이트
    return P_updated;
}

int main() {
    // 초기 상태 벡터 (x, y, theta, v)
    VectorXd x(STATE_DIM);
    x << 0, 0, 0, 1; // 임의의 초기값

    // 초기 오차 공분산 행렬
    MatrixXd P = MatrixXd::Identity(STATE_DIM, STATE_DIM);

    // 시간 간격
    double dt = 0.1; // 100ms

    // 측정 행렬 H (모바일 로봇에서 측정 가능한 값: x, y, theta)
    MatrixXd H(MEASURE_DIM, STATE_DIM);
    H << 1, 0, 0, 0,
         0, 1, 0, 0,
         0, 0, 1, 0;

    // 측정값 (예: 실제 측정값)
    VectorXd z(MEASURE_DIM);
    z << 1, 1, 0.1; // 임의의 측정값

    // 시스템 노이즈 공분산 (임의로 설정)
    processNoiseCovariance = MatrixXd::Identity(STATE_DIM, STATE_DIM) * 0.1;

    // 측정 잡음 공분산 (임의로 설정)
    measurementNoiseCovariance = MatrixXd::Identity(MEASURE_DIM, MEASURE_DIM) * 0.1;

    // 예측 단계
    VectorXd x_pred = predictState(x, dt);
    MatrixXd P_pred = predictCovariance(P, x, dt);

    // 칼만 이득 계산
    MatrixXd K = computeKalmanGain(P_pred, H);

    // 상태 업데이트
    VectorXd x_updated = updateState(x_pred, z, K, H);

    // 오차 공분산 업데이트
    MatrixXd P_updated = updateCovariance(P_pred, K, H);

    // 결과 출력
    std::cout << "Updated State: \n" << x_updated << std::endl;
    std::cout << "Updated Covariance: \n" << P_updated << std::endl;

    return 0;
}

상태 예측 (Predict State): predictState 함수는 모바일 로봇의 상태를 예측하는 함수이다. 선형 속도 $v$ 와 방향각 $\theta$ 를 사용하여 위치 $(x, y)$ 를 예측한다.
오차 공분산 예측 (Predict Covariance): predictCovariance 함수는 예측된 상태의 오차 공분산 행렬을 계산한다. 이는 시스템의 야코비 행렬 $F$ 를 기반으로 계산된다.
칼만 이득 계산 (Kalman Gain): computeKalmanGain 함수는 측정 행렬 $H$ 와 오차 공분산 행렬을 사용하여 칼만 이득을 계산한다.
상태 업데이트 (Update State): updateState 함수는 예측된 상태를 측정값을 사용하여 업데이트한다. 이는 예측값과 측정값의 차이(측정 오차)를 기반으로 한다.
오차 공분산 업데이트 (Update Covariance): updateCovariance 함수는 칼만 이득을 사용하여 오차 공분산 행렬을 업데이트한다.

이 코드는 모바일 로봇의 확장 칼만 필터를 구현한 것으로, 실제 로봇 시스템에 적용할 때는 측정 데이터와 시스템 노이즈 공분산을 실제 환경에 맞게 설정해야 한다.