자율 주행 차량 - 실험 도서관

개요

자율 주행 차량에서 확장 칼만 필터(EKF)는 다양한 센서 데이터를 결합하여 차량의 위치와 속도, 가속도 등을 정확히 추정하는 데 중요한 역할을 한다. 자율 주행 환경에서 발생하는 비선형적 동작을 처리하기 위해 EKF는 센서 데이터의 잡음 및 비선형성에 대한 적절한 필터링을 제공한다. 이 장에서는 자율 주행 차량에서 EKF가 어떻게 적용되는지에 대해 설명하고, 센서 융합과 위치 추정의 구체적인 예제를 제시한다.

시스템 모델링

자율 주행 차량에서의 상태 공간 모델(State Space Model)은 차량의 동작을 기술하는 수학적 표현이다. 일반적으로 자율 주행 차량의 상태는 위치, 속도, 가속도 등의 물리적 양으로 구성되며, 이를 벡터로 나타내면 다음과 같다:

$\mathbf{x}_k = \begin{bmatrix} x_k \\ y_k \\ \theta_k \\ v_k \end{bmatrix}$

여기서: - $x_k, y_k$ 는 각각 차량의 $k$ 번째 시간 단계에서의 위치, - $\theta_k$ 는 차량의 진행 방향 각도(Heading), - $v_k$ 는 차량의 속도를 의미한다.

시스템 동역학 모델

자율 주행 차량의 동역학 모델은 다음과 같은 비선형 방정식으로 표현된다:

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

여기서: - $f(\mathbf{x}_k, \mathbf{u}_k)$ 는 시스템의 상태 전이 함수로, 차량의 상태 변화를 기술하는 함수이다. - $\mathbf{u}_k$ 는 입력 벡터로, 조향 각도와 가속도를 포함한다. - $\mathbf{w}_k$ 는 잡음 항으로, 일반적으로 가우시안 분포를 따른다.

시스템 전이 함수 $f(\mathbf{x}_k, \mathbf{u}_k)$ 는 차량의 동작을 나타내며, 예를 들어 자율 주행 차량의 운동학 모델은 다음과 같은 형태로 표현될 수 있다:

$f(\mathbf{x}_k, \mathbf{u}_k) = \begin{bmatrix} x_k + \Delta t \cdot v_k \cdot \cos(\theta_k) \\ y_k + \Delta t \cdot v_k \cdot \sin(\theta_k) \\ \theta_k + \Delta t \cdot \frac{v_k}{L} \cdot \tan(\phi_k) \\ v_k + \Delta t \cdot a_k \end{bmatrix}$

여기서: - $\Delta t$ 는 시간 간격, - $L$ 은 차량의 휠베이스 길이, - $\phi_k$ 는 조향 각도, - $a_k$ 는 차량의 가속도를 나타낸다.

센서 모델링

자율 주행 차량은 여러 센서(예: GPS, LiDAR, IMU)로부터 데이터를 수집한다. 이러한 센서 데이터는 상태 추정을 위한 관측 모델에서 사용되며, 일반적으로 다음과 같은 형태로 주어진다:

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

여기서: - $h(\mathbf{x}_k)$ 는 관측 모델 함수로, 상태 벡터에서 센서 데이터로의 매핑을 나타낸다. - $\mathbf{v}_k$ 는 관측 잡음 항으로, 역시 가우시안 잡음을 따른다.

예를 들어 GPS를 통해 차량의 위치를 관측할 경우, 관측 모델은 다음과 같이 표현될 수 있다:

$h(\mathbf{x}_k) = \begin{bmatrix} x_k \\ y_k \end{bmatrix}$

또한 IMU(Inertial Measurement Unit)를 통해 차량의 가속도를 측정할 경우, 관측 모델은 다음과 같은 형태를 취한다:

$h(\mathbf{x}_k) = \begin{bmatrix} a_k \end{bmatrix}$

확장 칼만 필터 적용

자율 주행 차량에서의 EKF 적용은 크게 두 가지 단계로 이루어진다: 예측 단계(Prediction Step)와 업데이트 단계(Update Step).

예측 단계

예측 단계에서는 현재 상태 추정값을 바탕으로 다음 시간 단계에서의 상태를 예측한다. 예측 과정은 시스템 모델에 따라 상태 벡터와 공분산 행렬을 업데이트하는 방식으로 진행된다. 상태 예측은 다음과 같이 이루어진다:

$\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{F}_k = \frac{\partial f}{\partial \mathbf{x}} \bigg|_{\hat{\mathbf{x}}_{k|k}, \mathbf{u}_k}$ 는 상태 전이 함수의 야코비 행렬이다. - $\mathbf{Q}_k$ 는 시스템 노이즈 공분산 행렬이다.

업데이트 단계

업데이트 단계에서는 예측된 상태를 실제 관측된 센서 데이터와 비교하여 상태를 수정한다. 자율 주행 차량에서는 다양한 센서로부터 관측된 데이터를 활용하여 상태 추정의 정확성을 높인다. 업데이트 단계에서 관측 데이터에 따라 상태 벡터와 공분산 행렬이 다음과 같이 수정된다:

칼만 이득 계산

먼저, 칼만 이득 $\mathbf{K}_k$ 를 계산하여 센서로부터 얻은 관측 데이터의 신뢰도를 반영한다. 칼만 이득은 다음 식으로 계산된다:

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

여기서: - $\mathbf{H}_k = \frac{\partial h}{\partial \mathbf{x}} \bigg|_{\hat{\mathbf{x}}_{k|k-1}}$ 는 관측 모델의 야코비 행렬이다. - $\mathbf{R}_k$ 는 관측 잡음 공분산 행렬이다.

상태 벡터 업데이트

칼만 이득을 계산한 후, 실제 관측된 센서 데이터 $\mathbf{z}_k$ 와 예측된 관측값 $h(\hat{\mathbf{x}}_{k|k-1})$ 의 차이, 즉 잔차(Residual)를 이용해 상태 벡터를 업데이트한다:

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

잔차 $\mathbf{z}_k - h(\hat{\mathbf{x}}_{k|k-1})$ 는 관측값과 예측값의 차이를 의미하며, 칼만 이득 $\mathbf{K}_k$ 를 통해 상태 벡터에 반영된다.

공분산 행렬 업데이트

마지막으로, 공분산 행렬은 다음 식을 사용하여 업데이트된다:

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

여기서 $\mathbf{I}$ 는 항등 행렬이다. 이 업데이트 과정은 시스템의 상태 추정 오차를 줄이는 역할을 한다.

센서 융합

자율 주행 차량에서는 다양한 센서 데이터(GPS, IMU, LiDAR 등)를 융합하여 EKF를 통해 보다 정확한 상태 추정을 할 수 있다. 각각의 센서는 고유의 잡음 특성과 정확도를 가지므로, 센서 융합을 통해 각 센서의 장점을 극대화하고, 단점을 보완할 수 있다.

GPS 데이터와 EKF

GPS는 차량의 위치를 제공하지만, 업데이트 속도가 느리고 잡음이 크다. 이를 보완하기 위해 EKF는 GPS로부터 위치 데이터를 수신할 때마다 예측된 상태 벡터를 수정하고, GPS의 측정 잡음을 모델링하여 신뢰도를 반영한다.

IMU 데이터와 EKF

IMU는 가속도와 각속도 데이터를 제공하여 차량의 자세 및 가속도를 추정하는 데 사용된다. IMU는 매우 높은 업데이트 속도를 제공하지만, 장기적으로는 드리프트 문제가 발생할 수 있다. EKF는 IMU 데이터를 자주 업데이트하여 차량의 동작을 추정하면서, 다른 센서 데이터를 이용해 IMU의 드리프트 문제를 보완한다.

LiDAR 데이터와 EKF

LiDAR는 주변 환경을 스캔하여 차량 주변의 장애물 위치와 형태를 파악할 수 있다. EKF는 LiDAR 데이터를 융합하여 장애물 회피와 경로 계획에 활용되며, LiDAR 데이터를 사용하여 위치 추정의 정확성을 높일 수 있다.

자율 주행 차량에서 EKF의 이점

EKF는 자율 주행 차량에서의 비선형 시스템을 처리하는 데 매우 효과적이다. 다양한 센서 데이터를 융합하여 차량의 상태를 실시간으로 추정할 수 있으며, 이를 통해 보다 안전하고 정확한 자율 주행을 구현할 수 있다. EKF의 이점은 특히 차량의 위치 추정, 경로 추적, 장애물 회피 등에서 두드러지며, 자율 주행 시스템의 성능 향상에 크게 기여한다.

C++ Eigen 구현

자율 주행 차량에서 확장 칼만 필터(EKF)를 C++ 및 Eigen 라이브러리로 구현하는 방법을 단계별로 설명하겠다. 예제는 위에서 설명한 칼만 필터의 예측 및 업데이트 단계, 그리고 GPS 및 IMU 데이터를 융합하여 상태를 추정하는 방식에 중점을 둔다.

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

using namespace Eigen;

// 상태 벡터의 차원
const int state_dim = 4;   // [x, y, theta, v]
const int control_dim = 2; // [steering angle, acceleration]
const int measurement_dim = 2; // GPS [x, y]

// 필터 변수 정의
VectorXd x(state_dim);      // 상태 벡터
MatrixXd P(state_dim, state_dim);  // 공분산 행렬
MatrixXd F(state_dim, state_dim);  // 상태 전이 행렬
MatrixXd Q(state_dim, state_dim);  // 시스템 노이즈 공분산 행렬
MatrixXd H(measurement_dim, state_dim);  // 관측 모델 행렬
MatrixXd R(measurement_dim, measurement_dim);  // 관측 잡음 공분산 행렬
MatrixXd K(state_dim, measurement_dim);  // 칼만 이득
VectorXd z(measurement_dim);  // 관측 벡터

// 상태 전이 함수
VectorXd f(const VectorXd& x, const VectorXd& u, double dt) {
    VectorXd x_next(state_dim);
    double theta = x(2);
    double v = x(3);
    double steering_angle = u(0);
    double acceleration = u(1);
    double L = 2.5; // 차량의 휠베이스 (예시 값)

    x_next(0) = x(0) + dt * v * cos(theta); // x 위치
    x_next(1) = x(1) + dt * v * sin(theta); // y 위치
    x_next(2) = x(2) + dt * (v / L) * tan(steering_angle); // 진행 방향(헤딩) 각도
    x_next(3) = x(3) + dt * acceleration; // 속도

    return x_next;
}

// 상태 전이 함수의 야코비 계산
MatrixXd calculateJacobianF(const VectorXd& x, const VectorXd& u, double dt) {
    MatrixXd F(state_dim, state_dim);
    double theta = x(2);
    double v = x(3);
    double steering_angle = u(0);
    double L = 2.5; // 휠베이스

    F.setIdentity();
    F(0, 2) = -dt * v * sin(theta); // ∂f/∂theta
    F(0, 3) = dt * cos(theta); // ∂f/∂v
    F(1, 2) = dt * v * cos(theta); // ∂f/∂theta
    F(1, 3) = dt * sin(theta); // ∂f/∂v
    F(2, 3) = dt / L * tan(steering_angle); // ∂f/∂v

    return F;
}

// 예측 단계
void predict(const VectorXd& u, double dt) {
    // 상태 벡터 예측
    x = f(x, u, dt);

    // 야코비 행렬 계산 및 공분산 예측
    F = calculateJacobianF(x, u, dt);
    P = F * P * F.transpose() + Q;
}

// 관측 모델 함수 (GPS)
VectorXd h(const VectorXd& x) {
    VectorXd z_hat(measurement_dim);
    z_hat(0) = x(0); // x 위치
    z_hat(1) = x(1); // y 위치
    return z_hat;
}

// 관측 모델의 야코비 계산
MatrixXd calculateJacobianH() {
    MatrixXd H(measurement_dim, state_dim);
    H.setZero();
    H(0, 0) = 1.0; // ∂h/∂x
    H(1, 1) = 1.0; // ∂h/∂y
    return H;
}

// 업데이트 단계
void update(const VectorXd& z) {
    // 예측된 관측값 계산
    VectorXd z_hat = h(x);

    // 잔차 계산
    VectorXd y = z - z_hat;

    // 칼만 이득 계산
    H = calculateJacobianH();
    MatrixXd S = H * P * H.transpose() + R; // S = HP(H^T) + R
    K = P * H.transpose() * S.inverse();   // 칼만 이득

    // 상태 벡터 업데이트
    x = x + K * y;

    // 공분산 행렬 업데이트
    MatrixXd I = MatrixXd::Identity(state_dim, state_dim);
    P = (I - K * H) * P;
}

int main() {
    // 초기화
    x << 0, 0, 0, 1; // [x, y, theta, v]
    P.setIdentity(); // 초기 공분산 행렬
    Q.setIdentity(); // 시스템 노이즈 공분산 행렬
    R.setIdentity(); // 관측 잡음 공분산 행렬

    // 예측 단계
    VectorXd u(control_dim);
    u << 0.1, 0.2; // [steering angle, acceleration]
    double dt = 0.1; // 시간 간격
    predict(u, dt);

    // 관측 데이터 (예: GPS 데이터)
    z << 1.0, 1.5; // [x, y]
    update(z);

    // 결과 출력
    std::cout << "Updated state: \n" << x << std::endl;
    std::cout << "Updated covariance: \n" << P << std::endl;

    return 0;
}

상태 벡터 (x)는 차량의 위치 $x$ , $y$ , 진행 방향 각도 $\theta$ , 속도 $v$ 로 구성된다.
상태 전이 함수 (f)는 차량의 물리적 모델에 따라 상태를 예측한다. 차량의 위치와 방향은 $\theta$ , 속도 $v$ , 조향 각도 $\phi$ , 가속도 $a$ 에 따라 계산된다.
상태 전이 행렬의 야코비 (F)은 비선형 상태 전이를 선형 근사하기 위한 행렬이다.
예측 단계는 상태 벡터와 공분산 행렬을 예측하는 단계이다.
업데이트 단계는 실제 센서 데이터(GPS)를 사용하여 상태 벡터와 공분산 행렬을 수정한다.
칼만 이득 (K)을 통해 잔차를 기반으로 상태 벡터를 업데이트하고, 공분산 행렬을 갱신한다.

이 코드는 자율 주행 차량에서 확장 칼만 필터를 구현하는 예제이다. IMU와 같은 다른 센서 데이터를 추가하는 경우, 관측 모델과 해당 야코비 행렬을 확장하면 된다.