센서 노이즈 시뮬레이션

로봇 시뮬레이션에서 센서 노이즈는 현실 세계의 불확실성과 측정 오차를 반영하는 중요한 요소이다. 센서 노이즈를 정확하게 모델링함으로써 시뮬레이션의 신뢰성을 높이고, 로봇의 제어 알고리즘과 센서 처리 로직을 실제 환경에 가깝게 테스트할 수 있다. 이 장에서는 센서 노이즈의 종류, 수학적 모델링 방법, Unity에서의 구현 방법에 대해 자세히 다룬다.

센서 노이즈의 종류

센서 노이즈는 다양한 형태로 나타날 수 있으며, 각 센서의 특성에 따라 다르게 모델링된다. 일반적으로 다음과 같은 노이즈 유형이 존재한다:

백색 가우시안 노이즈 (White Gaussian Noise):
가장 일반적으로 사용되는 노이즈 모델로, 평균이 0이고 분산이 일정한 가우시안 분포를 따른다.
대부분의 센서에서 관찰되는 랜덤한 측정 오차를 근사적으로 표현할 수 있다.
바이어스 노이즈 (Bias Noise):
센서의 지속적인 오차로, 측정값에 일정한 편향을 추가한다.
예를 들어, IMU 센서의 경우 지속적인 가속도나 각속도 편향이 발생할 수 있다.
드리프트 노이즈 (Drift Noise):
시간이 지남에 따라 누적되는 오차로, 센서의 측정값이 점차적으로 변하는 현상을 나타낸다.
예를 들어, 자이로스코프의 드리프트는 로봇의 자세 추정에 영향을 미칠 수 있다.
포아송 노이즈 (Poisson Noise):
주로 광 센서나 카메라와 같은 센서에서 발생하며, 이벤트의 발생 횟수가 확률적으로 변하는 경우에 사용된다.
광자의 검출과 관련된 노이즈를 모델링할 때 유용하다.

수학적 노이즈 모델링

센서 노이즈를 수학적으로 모델링하기 위해서는 각 노이즈 유형에 맞는 확률 분포를 사용한다. 가장 일반적인 가우시안 노이즈 모델을 예로 들어보겠다.

가우시안 노이즈

가우시안 노이즈는 다음과 같은 형태로 모델링된다:

$n \sim \mathcal{N}(0, \sigma^2)$

여기서 $n$ 은 노이즈, $\mathcal{N}$ 은 정규 분포, 평균은 0, 분산은 $\sigma^2$ 이다.

센서의 실제 측정값 $z$ 는 실제 값 $x$ 와 노이즈 $n$ 의 합으로 표현된다:

$z = x + n$

바이어스 노이즈

바이어스 노이즈는 일정한 값 $b$ 를 측정값에 추가하여 모델링된다:

$z = x + b$

여기서 $b$ 는 센서의 바이어스이다.

드리프트 노이즈

드리프트 노이즈는 시간이 지남에 따라 누적되는 형태로 모델링된다. 예를 들어, 시간 $t$ 에 따라 선형적으로 증가하는 드리프트를 다음과 같이 표현할 수 있다:

$z(t) = x + b \cdot t$

여기서 $b$ 는 드리프트 속도이다.

Unity에서의 노이즈 구현

Unity에서 센서 노이즈를 구현하기 위해서는 주로 C# 스크립트를 사용하여 센서 데이터에 노이즈를 추가한다. 다음은 간단한 가우시안 노이즈를 센서 데이터에 추가하는 예제이다.

using UnityEngine;

public class SensorNoise
{
    private System.Random random;
    private float mean;
    private float stdDev;

    public SensorNoise(float mean = 0f, float stdDev = 1f)
    {
        this.mean = mean;
        this.stdDev = stdDev;
        random = new System.Random();
    }

    public float AddGaussianNoise(float value)
    {
        // Box-Muller 변환을 사용하여 가우시안 노이즈 생성
        double u1 = 1.0 - random.NextDouble();
        double u2 = 1.0 - random.NextDouble();
        double randStdNormal = Mathf.Sqrt(-2.0f * Mathf.Log((float)u1)) *
                               Mathf.Sin(2.0f * Mathf.PI * (float)u2);
        double noise = mean + stdDev * randStdNormal;
        return value + (float)noise;
    }
}

이 스크립트는 센서 데이터에 가우시안 노이즈를 추가하는 기능을 제공한다. mean과 stdDev 파라미터를 조정하여 노이즈의 특성을 변경할 수 있다.

다양한 센서에 대한 노이즈 모델링

각 센서마다 특성에 맞는 노이즈 모델링이 필요하다. 다음은 대표적인 센서들에 대한 노이즈 모델링 방법이다.

라이다(LiDAR) 센서

라이다 센서는 거리 측정 시 다양한 오차가 발생할 수 있다. 주로 거리 측정값에 가우시안 노이즈를 추가하여 모델링한다.

$z_{\text{LiDAR}} = d + n$

여기서 $d$ 는 실제 거리, $n \sim \mathcal{N}(0, \sigma_d^2)$ 는 거리 노이즈이다.

또한, 라이다의 각도 측정에도 노이즈가 있을 수 있다:

$\theta_{\text{LiDAR}} = \theta + n_{\theta}$

카메라 센서

카메라 센서는 픽셀 값에 노이즈가 추가될 수 있다. 주로 포아송 노이즈나 가우시안 노이즈를 사용하여 이미지에 노이즈를 추가한다.

public Color AddGaussianNoiseToColor(Color color, float mean, float stdDev)
{
    float r = color.r + gaussianNoise(mean, stdDev);
    float g = color.g + gaussianNoise(mean, stdDev);
    float b = color.b + gaussianNoise(mean, stdDev);
    return new Color(Mathf.Clamp(r, 0f, 1f), Mathf.Clamp(g, 0f, 1f), Mathf.Clamp(b, 0f, 1f));
}

private float gaussianNoise(float mean, float stdDev)
{
    // Box-Muller 변환을 사용하여 가우시안 노이즈 생성
    double u1 = 1.0 - random.NextDouble();
    double u2 = 1.0 - random.NextDouble();
    double randStdNormal = Mathf.Sqrt(-2.0f * Mathf.Log((float)u1)) *
                           Mathf.Sin(2.0f * Mathf.PI * (float)u2);
    return mean + stdDev * (float)randStdNormal;
}

IMU 센서

IMU 센서는 가속도계와 자이로스코프 데이터를 제공한다. 이 데이터에는 가우시안 노이즈와 바이어스 노이즈가 주로 추가된다.

$\mathbf{a}_{\text{IMU}} = \mathbf{a}_{\text{true}} + \mathbf{n}_a + \mathbf{b}_a$

$\boldsymbol{\omega}_{\text{IMU}} = \boldsymbol{\omega}_{\text{true}} + \mathbf{n}_\omega + \mathbf{b}_\omega$

여기서 $\mathbf{n}_a$ 와 $\mathbf{n}_\omega$ 는 가우시안 노이즈, $\mathbf{b}_a$ 와 $\mathbf{b}_\omega$ 는 바이어스 노이즈이다.

GPS 센서

GPS(Global Positioning System) 센서는 로봇의 위치를 외부 환경에서 추정하는 데 사용된다. GPS 센서의 노이즈는 주로 위치 측정의 정확도에 영향을 미치며, 다양한 요인에 의해 발생할 수 있다. GPS 센서 노이즈 모델링에는 다음과 같은 요소들이 포함된다:

$\mathbf{p}_{\text{GPS}} = \mathbf{p}_{\text{true}} + \mathbf{n}_{\text{GPS}}$

여기서 $\mathbf{p}_{\text{GPS}}$ 는 GPS 센서에서 측정된 위치, $\mathbf{p}_{\text{true}}$ 는 실제 위치, $\mathbf{n}_{\text{GPS}}$ 는 GPS 노이즈이다. GPS 노이즈는 주로 가우시안 분포를 따르지만, 환경에 따라 비가우시안 노이즈가 발생할 수 있다.

GPS 노이즈 구현 예제

Unity에서 GPS 노이즈를 구현하기 위해서는 위치 데이터에 가우시안 노이즈를 추가하는 방식으로 접근할 수 있다. 다음은 C#을 사용하여 GPS 노이즈를 추가하는 예제이다.

using UnityEngine;

public class GPSNoise
{
    private System.Random random;
    private float stdDev;

    public GPSNoise(float stdDev = 1.0f)
    {
        this.stdDev = stdDev;
        random = new System.Random();
    }

    public Vector3 AddNoise(Vector3 truePosition)
    {
        float noiseX = GaussianNoise(0f, stdDev);
        float noiseY = GaussianNoise(0f, stdDev);
        float noiseZ = GaussianNoise(0f, stdDev);
        return new Vector3(truePosition.x + noiseX, truePosition.y + noiseY, truePosition.z + noiseZ);
    }

    private float GaussianNoise(float mean, float stdDev)
    {
        double u1 = 1.0 - random.NextDouble();
        double u2 = 1.0 - random.NextDouble();
        double randStdNormal = Mathf.Sqrt(-2.0f * Mathf.Log((float)u1)) *
                               Mathf.Sin(2.0f * Mathf.PI * (float)u2);
        return mean + stdDev * (float)randStdNormal;
    }
}

이 스크립트는 실제 위치에 가우시안 노이즈를 추가하여 GPS 센서의 측정값을 시뮬레이션한다. stdDev 파라미터를 조정하여 노이즈의 표준 편차를 설정할 수 있다.

초음파(Ultrasonic) 센서

초음파 센서는 물체와의 거리를 측정하는 데 사용되며, 주로 로봇의 장애물 감지에 활용된다. 초음파 센서의 노이즈는 거리 측정의 정확도에 영향을 미치며, 주로 다음과 같은 형태로 나타난다:

$d_{\text{US}} = d_{\text{true}} + n_{\text{US}}$

여기서 $d_{\text{US}}$ 는 초음파 센서에서 측정된 거리, $d_{\text{true}}$ 는 실제 거리, $n_{\text{US}}$ 는 초음파 센서 노이즈이다. 초음파 노이즈는 주로 가우시안 노이즈로 모델링되지만, 반사 표면의 특성에 따라 비가우시안 노이즈가 발생할 수 있다.

초음파 센서 노이즈 구현 예제

Unity에서 초음파 센서 노이즈를 구현하기 위해서는 거리 데이터에 가우시안 노이즈를 추가한다. 다음은 C#을 사용한 구현 예제이다.

using UnityEngine;

public class UltrasonicNoise
{
    private System.Random random;
    private float stdDev;

    public UltrasonicNoise(float stdDev = 0.05f)
    {
        this.stdDev = stdDev;
        random = new System.Random();
    }

    public float AddNoise(float trueDistance)
    {
        float noise = GaussianNoise(0f, stdDev);
        return trueDistance + noise;
    }

    private float GaussianNoise(float mean, float stdDev)
    {
        double u1 = 1.0 - random.NextDouble();
        double u2 = 1.0 - random.NextDouble();
        double randStdNormal = Mathf.Sqrt(-2.0f * Mathf.Log((float)u1)) *
                               Mathf.Sin(2.0f * Mathf.PI * (float)u2);
        return mean + stdDev * (float)randStdNormal;
    }
}

이 스크립트는 초음파 센서의 실제 거리 측정값에 가우시안 노이즈를 추가하여 시뮬레이션한다. stdDev 파라미터를 통해 노이즈의 표준 편차를 설정할 수 있다.

온도 센서

온도 센서는 로봇의 환경 온도를 측정하는 데 사용된다. 온도 센서의 노이즈는 주로 다음과 같이 모델링된다:

$T_{\text{measured}} = T_{\text{true}} + n_T$

여기서 $T_{\text{measured}}$ 는 측정된 온도, $T_{\text{true}}$ 는 실제 온도, $n_T$ 는 온도 센서 노이즈이다. 온도 노이즈는 주로 가우시안 분포를 따르지만, 특정 환경 조건에서는 비가우시안 노이즈가 발생할 수 있다.

온도 센서 노이즈 구현 예제

Unity에서 온도 센서 노이즈를 구현하기 위해서는 온도 데이터에 가우시안 노이즈를 추가한다. 다음은 C#을 사용한 구현 예제이다.

using UnityEngine;

public class TemperatureNoise
{
    private System.Random random;
    private float stdDev;

    public TemperatureNoise(float stdDev = 0.1f)
    {
        this.stdDev = stdDev;
        random = new System.Random();
    }

    public float AddNoise(float trueTemperature)
    {
        float noise = GaussianNoise(0f, stdDev);
        return trueTemperature + noise;
    }

    private float GaussianNoise(float mean, float stdDev)
    {
        double u1 = 1.0 - random.NextDouble();
        double u2 = 1.0 - random.NextDouble();
        double randStdNormal = Mathf.Sqrt(-2.0f * Mathf.Log((float)u1)) *
                               Mathf.Sin(2.0f * Mathf.PI * (float)u2);
        return mean + stdDev * (float)randStdNormal;
    }
}

이 스크립트는 온도 센서의 실제 온도 측정값에 가우시안 노이즈를 추가하여 시뮬레이션한다. stdDev 파라미터를 조정하여 노이즈의 표준 편차를 설정할 수 있다.

센서 융합과 노이즈 처리

로봇은 종종 여러 센서로부터 데이터를 수집하며, 이러한 데이터를 융합하여 보다 정확한 상태 추정을 수행한다. 센서 융합 과정에서 노이즈는 중요한 고려 사항이다. 다음은 센서 융합과 노이즈 처리에 대한 기본적인 접근 방식이다:

칼만 필터(Kalman Filter)

칼만 필터는 센서 데이터의 노이즈를 줄이고, 여러 센서로부터의 데이터를 효율적으로 융합하기 위한 대표적인 알고리즘이다. 칼만 필터는 예측 단계와 업데이트 단계를 통해 상태 추정을 수행한다.

예측 단계:

$\mathbf{\hat{x}}_{k|k-1} = \mathbf{A} \mathbf{\hat{x}}_{k-1|k-1} + \mathbf{B} \mathbf{u}_k$

$\mathbf{P}_{k|k-1} = \mathbf{A} \mathbf{P}_{k-1|k-1} \mathbf{A}^\top + \mathbf{Q}$

업데이트 단계:

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

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

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

여기서 $\mathbf{\hat{x}}$ 는 상태 추정, $\mathbf{P}$ 는 오차 공분산, $\mathbf{A}$ 는 상태 전이 행렬, $\mathbf{B}$ 는 제어 입력 행렬, $\mathbf{u}$ 는 제어 입력, $\mathbf{Q}$ 는 시스템 노이즈 공분산, $\mathbf{K}$ 는 칼만 이득, $\mathbf{H}$ 는 관측 행렬, $\mathbf{R}$ 은 관측 노이즈 공분산, $\mathbf{z}$ 는 측정값, $\mathbf{I}$ 는 항등 행렬이다.

칼만 필터를 Unity에서 구현하여 여러 센서로부터의 노이즈 데이터를 효과적으로 처리할 수 있다.

using UnityEngine;

public class KalmanFilter
{
    private float Q; // 시스템 노이즈 공분산
    private float R; // 관측 노이즈 공분산
    private float P; // 오차 공분산
    private float K; // 칼만 이득
    private float X; // 상태 추정

    public KalmanFilter(float processNoise, float measurementNoise, float initialEstimate, float initialError)
    {
        Q = processNoise;
        R = measurementNoise;
        X = initialEstimate;
        P = initialError;
    }

    public float Update(float measurement)
    {
        // 예측 단계
        P = P + Q;

        // 업데이트 단계
        K = P / (P + R);
        X = X + K * (measurement - X);
        P = (1 - K) * P;

        return X;
    }
}

이 스크립트는 단일 변수 칼만 필터를 구현한 것으로, 센서로부터의 측정값을 받아 상태를 업데이트한다. 여러 센서의 데이터를 융합하려면 다변수 칼만 필터로 확장할 수 있다.