카메라, LIDAR, IMU 데이터 수집

카메라 데이터 수집

카메라 데이터 수집은 주로 시뮬레이션에서 환경의 이미지를 얻어 후처리하거나 분석하는 데 사용된다. 시뮬레이션에서 카메라는 3D 공간에서 위치와 방향을 가진다. 카메라의 주요 매개변수로는 초점 거리, 센서 크기, 해상도 등이 있다. 이를 URDF나 SDF 파일에서 정의하고 Gazebo와 같은 시뮬레이션 환경에서 데이터를 수집할 수 있다.

카메라 모델을 정의할 때 중요한 요소는 다음과 같다:

카메라 파라미터

카메라 모델의 기본 방정식은 핀홀 카메라 모델로 설명할 수 있으며, 이는 3D 세계 좌표에서 2D 이미지 평면으로의 변환을 나타낸다. 이를 수학적으로 나타내면:

$\mathbf{p}_{\text{image}} = \mathbf{K} \mathbf{T} \mathbf{P}_{\text{world}}$

여기서: - $\mathbf{p}_{\text{image}}$ 는 2D 이미지 좌표이다. - $\mathbf{K}$ 는 카메라 내부 파라미터 행렬이다.

$\mathbf{K} = \begin{bmatrix} f_x & 0 & c_x \\ 0 & f_y & c_y \\ 0 & 0 & 1 \end{bmatrix}$

여기서 $f_x$ 와 $f_y$ 는 이미지 평면의 초점 거리, $c_x$ 와 $c_y$ 는 이미지 센터 좌표이다.

$\mathbf{T}$ 는 카메라의 외부 파라미터 행렬로, 회전 및 변환을 포함한다.
$\mathbf{P}_{\text{world}}$ 는 3D 세계 좌표에서의 포인트이다.

LIDAR 데이터 수집

LIDAR는 레이저를 사용하여 주변 환경의 거리를 측정하고, 이를 통해 3D 점군 데이터를 생성한다. 시뮬레이션에서는 LIDAR 센서를 정의하여 환경의 구조를 모델링할 수 있다. LIDAR의 주요 매개변수로는 레이저 스캔 각도, 레이저 거리 범위, 해상도 등이 있다.

LIDAR 센서가 제공하는 데이터는 주로 거리 값으로, 이를 수학적으로는 다음과 같이 나타낼 수 있다:

$d_i = \sqrt{(x_i - x_0)^2 + (y_i - y_0)^2 + (z_i - z_0)^2}$

여기서: - $d_i$ 는 LIDAR 센서에서 측정한 거리이다. - $(x_0, y_0, z_0)$ 는 LIDAR 센서의 위치이다. - $(x_i, y_i, z_i)$ 는 측정된 물체의 좌표이다.

IMU 데이터 수집

IMU(Inertial Measurement Unit)는 가속도계와 자이로스코프를 사용하여 로봇의 자세와 속도를 측정한다. IMU 센서에서 제공되는 데이터는 주로 3축 가속도와 3축 각속도이다.

IMU 데이터는 다음과 같은 수식으로 표현할 수 있다:

가속도 데이터:

$\mathbf{a}_{\text{measured}} = \mathbf{a}_{\text{world}} + \mathbf{g}$

여기서: - $\mathbf{a}_{\text{measured}}$ 는 IMU에서 측정한 가속도이다. - $\mathbf{a}_{\text{world}}$ 는 월드 좌표계에서의 가속도이다. - $\mathbf{g}$ 는 중력 가속도 벡터이다.

각속도 데이터:

$\boldsymbol{\omega}_{\text{measured}} = \boldsymbol{\omega}_{\text{world}}$

여기서: - $\boldsymbol{\omega}_{\text{measured}}$ 는 IMU에서 측정한 각속도이다. - $\boldsymbol{\omega}_{\text{world}}$ 는 월드 좌표계에서의 각속도이다.

IMU 센서는 자주 Kalman 필터나 다른 필터링 기법을 사용하여 데이터에서 노이즈를 제거하고 정확한 자세 추정을 수행한다.

카메라, LIDAR, IMU 데이터 수집을 위한 URDF/SDF 설정

시뮬레이션에서 센서를 정의하고 데이터 수집을 수행하기 위해서는 URDF 또는 SDF 파일에 각 센서의 특성과 파라미터를 설정해야 한다. 여기서는 각 센서의 URDF/SDF 설정 방법에 대해 다룬다.

카메라 센서의 URDF/SDF 설정

URDF와 SDF에서 카메라 센서를 정의하려면 카메라의 위치, 방향, 해상도, 초점 거리 등의 파라미터를 설정해야 한다. 예를 들어, 카메라의 해상도는 SDF 파일에서 다음과 같이 설정할 수 있다:

<sensor name="camera" type="camera">
  <camera>
    <horizontal_fov>1.3962634</horizontal_fov>
    <image>
      <width>640</width>
      <height>480</height>
      <format>R8G8B8</format>
    </image>
    <clip>
      <near>0.1</near>
      <far>100</far>
    </clip>
  </camera>
</sensor>

여기서 horizontal_fov는 카메라의 수평 시야각이며, width와 height는 이미지의 해상도를 정의한다. clip은 카메라의 클리핑 평면을 정의하여 카메라가 볼 수 있는 최소 및 최대 거리를 설정한다.

LIDAR 센서의 URDF/SDF 설정

LIDAR 센서는 URDF나 SDF에서 설정할 수 있으며, 특히 레이저의 범위, 해상도, 스캔 각도 등을 정의해야 한다. SDF 파일에서 LIDAR 센서를 설정하는 예시는 다음과 같다:

<sensor name="lidar" type="gpu_ray">
  <ray>
    <scan>
      <horizontal>
        <samples>640</samples>
        <resolution>1</resolution>
        <min_angle>-1.5708</min_angle>
        <max_angle>1.5708</max_angle>
      </horizontal>
      <vertical>
        <samples>1</samples>
        <resolution>1</resolution>
        <min_angle>0</min_angle>
        <max_angle>0</max_angle>
      </vertical>
    </scan>
    <range>
      <min>0.1</min>
      <max>30.0</max>
      <resolution>0.01</resolution>
    </range>
  </ray>
</sensor>

여기서 samples는 레이저 빔의 수를 나타내고, min_angle과 max_angle은 레이저가 스캔하는 각도를 정의한다. min과 max는 LIDAR의 거리 범위를 나타내며, resolution은 거리 해상도를 설정한다.

IMU 센서의 URDF/SDF 설정

IMU는 가속도계와 자이로스코프의 데이터를 수집하는 센서로, URDF나 SDF에서 설정할 수 있다. 다음은 SDF에서 IMU 센서를 정의하는 예시이다:

<sensor name="imu" type="imu">
  <imu>
    <angular_velocity>
      <x>0</x>
      <y>0</y>
      <z>0</z>
    </angular_velocity>
    <linear_acceleration>
      <x>0</x>
      <y>0</y>
      <z>0</z>
    </linear_acceleration>
  </imu>
</sensor>

여기서 angular_velocity는 자이로스코프 데이터를 나타내고, linear_acceleration은 가속도계 데이터를 나타낸다. IMU 센서는 로봇의 회전과 가속도를 추적하여 자세 추정에 중요한 역할을 한다.

센서 데이터 필터링

센서 데이터는 노이즈가 포함될 수 있으며, 이를 처리하기 위해 필터링 기법이 사용된다. 일반적으로 Kalman 필터, 확장 칼만 필터(EKF), 또는 복합 센서 필터링 기법이 활용된다. 특히 IMU 데이터를 처리할 때 가속도와 각속도 데이터를 동시에 처리하여 자세 추정의 정확성을 높일 수 있다.

확장 칼만 필터(EKF)

IMU 데이터는 확장 칼만 필터(EKF)를 통해 처리될 수 있다. EKF는 비선형 시스템을 다루는 데 적합하며, 센서 데이터를 결합하여 상태를 추정하는 데 사용된다. EKF에서 상태 벡터 $\mathbf{x}$ 는 다음과 같이 정의된다:

$\mathbf{x} = \begin{bmatrix} \mathbf{p} \\ \mathbf{v} \\ \mathbf{q} \\ \boldsymbol{\omega} \end{bmatrix}$

여기서: - $\mathbf{p}$ 는 위치 벡터이다. - $\mathbf{v}$ 는 속도 벡터이다. - $\mathbf{q}$ 는 쿼터니언으로 나타낸 자세 벡터이다. - $\boldsymbol{\omega}$ 는 각속도 벡터이다.

EKF의 업데이트 단계는 다음과 같이 표현된다:

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

여기서: - $\mathbf{x}_{k+1}$ 는 다음 상태 벡터이다. - $f(\mathbf{x}_k, \mathbf{u}_k)$ 는 상태 변환 함수이다. - $\mathbf{u}_k$ 는 제어 입력 벡터이다. - $\mathbf{w}_k$ 는 시스템 노이즈이다.

관측 벡터 $\mathbf{z}$ 는 센서 데이터로부터 추정되며, 다음과 같이 표현된다:

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

여기서: - $h(\mathbf{x}_k)$ 는 관측 모델이다. - $\mathbf{v}_k$ 는 측정 노이즈이다.

EKF의 필터링 과정은 예측과 업데이트 단계로 구성된다. 이를 통해 센서 데이터의 노이즈를 제거하고 보다 정확한 상태 추정을 수행할 수 있다.