# 시뮬레이션 월드 만들기

자율주행 로봇을 개발하는 과정에서 시뮬레이션 월드 환경을 구성하는 것은 매우 중요하다. 시뮬레이션을 통해 물리적 실험을 대체할 수 있으며, 안전하게 다양한 조건에서 로봇의 성능을 평가하고 최적화할 수 있다. ROS2 Humble에서 시뮬레이션 환경을 구축하기 위해 Gazebo와 같은 시뮬레이션 툴을 활용할 수 있다. 아래에서는 시뮬레이션 월드를 생성하는 주요 단계와 고려 사항을 설명하겠다.

#### Gazebo 시뮬레이터 설정

ROS2와 통합된 Gazebo는 물리 엔진과 시각적 환경을 제공하여 현실감 있는 시뮬레이션을 가능하게 한다. Gazebo에서 월드를 구성하기 위해 다음과 같은 파일과 개념이 필요하다:

* **SDF (Simulation Description Format)**: 월드를 정의하는 파일 형식으로, 로봇 모델과 월드 환경을 XML로 기술한다.
* **URDF (Unified Robot Description Format)**: 로봇의 구조와 물리적 속성을 정의하며, SDF 파일과 호환되어 Gazebo와 ROS2의 인터페이스 역할을 한다.
* **환경 요소**: 빌딩, 장애물, 도로 등과 같은 시뮬레이션 공간 내의 물리적 객체들을 추가하여 현실감을 높일 수 있다.

#### SDF 파일 구조

SDF 파일은 기본적으로 XML 문서 구조를 따르며, 시뮬레이션 월드를 정의하는데 필요한 정보를 포함한다. 이 파일에서 환경의 물리적 특성, 중력, 마찰 계수 등을 설정할 수 있다.

```xml
<?xml version="1.0" ?>
<sdf version="1.6">
  <world name="default">
    <gravity>0 0 -9.8</gravity>
    <scene>
      <ambient>0.4 0.4 0.4 1</ambient>
      <background>0.7 0.7 0.9 1</background>
    </scene>
    <!-- 추가 객체를 여기서 정의 -->
  </world>
</sdf>
```

위 예제는 기본 월드를 설정하며, 중력과 조명 설정을 포함한다. 각 요소의 역할을 구체적으로 설명하겠다.

**중력 설정**

중력 설정은 월드 내에서 작용하는 중력 가속도를 정의한다. 일반적으로 지구 표준 중력값 $g = 9.8 , \text{m/s}^2$를 설정한다. 이를 통해 물체들이 물리적 상호작용을 할 때 자연스럽게 중력을 받도록 한다.

$$
\mathbf{g} = \begin{bmatrix} 0 \ 0 \ -9.8 \end{bmatrix} \text{ m/s}^2
$$

**조명과 색상 설정**

`<ambient>`와 `<background>` 태그는 시뮬레이션의 조명과 배경 색상을 정의한다. `<ambient>`는 환경 전체에 적용될 기본 조명 색상을 설정하며, `<background>`는 시뮬레이션 화면의 배경 색상을 정의한다. 이를 통해 로봇의 센서 테스트나 가시성 테스트에 유리한 환경을 설정할 수 있다.

#### 로봇 모델 추가하기

시뮬레이션 환경 내에서 로봇 모델을 포함하려면 URDF 파일을 정의한 후 이를 SDF 파일에 연결해야 한다. 아래는 ROS2에서 URDF 파일을 포함하는 예제이다.

```xml
<model name="robot_model">
  <include>
    <uri>model://my_robot</uri>
  </include>
</model>
```

**로봇 위치 설정**

로봇의 시작 위치를 설정할 수 있으며, 이는 `<pose>` 태그로 정의된다. 기본적인 초기 위치와 회전값을 정의할 수 있으며, 이는 행렬로 나타낼 수 있다. 로봇의 위치는 다음과 같이 나타낼 수 있다.

$$
\mathbf{P} = \begin{bmatrix} x \ y \ z \end{bmatrix}, \quad \mathbf{R} = \begin{bmatrix} \cos \theta & -\sin \theta & 0 \ \sin \theta & \cos \theta & 0 \ 0 & 0 & 1 \end{bmatrix}
$$

여기서 $\theta$는 초기 회전각이며, 이로 인해 로봇의 초기 방향이 설정된다.

#### 물리 엔진 설정

Gazebo에서 물리 엔진의 설정을 통해 시뮬레이션의 물리적 정확도를 조정할 수 있다. 이를 통해 다양한 실험을 가능하게 한다. 물리 엔진은 중력, 마찰 계수, 시간 스텝 등을 조정하며, 이를 SDF 파일에서 `<physics>` 태그로 설정한다.

```xml
<physics type="ode">
  <max_step_size>0.001</max_step_size>
  <real_time_factor>1.0</real_time_factor>
  <real_time_update_rate>1000.0</real_time_update_rate>
</physics>
```

**시간 스텝과 실시간 비율**

`<max_step_size>`는 시뮬레이션의 시간 스텝을 설정한다. 이를 통해 물리적 상호작용의 정확도를 조정할 수 있다. `real_time_factor`는 시뮬레이션 속도를 실제 시간에 맞추기 위한 비율을 정의한다.

$$
\Delta t = \text{max\_step\_size}
$$

이와 같이 시뮬레이션의 시간 스텝과 실시간 비율을 조정하여, 실제 환경과 유사한 조건을 구현할 수 있다.

#### 시뮬레이션 월드에 환경 요소 추가하기

시뮬레이션 환경을 더욱 현실감 있게 만들기 위해 다양한 물체와 환경 요소를 추가할 수 있다. Gazebo에서 제공하는 라이브러리에서 객체를 가져오거나 직접 모델링할 수 있다. 객체의 위치, 크기, 색상, 물리적 특성 등을 정의하여 시뮬레이션 환경을 구성한다.

**기본 객체 추가**

기본적인 객체(예: 상자, 원통, 구)를 추가하려면 SDF 파일에서 `<model>` 태그를 사용하여 해당 객체를 정의한다. 각 객체의 물리적 특성과 위치를 설정하여 시뮬레이션에 포함할 수 있다.

```xml
<model name="box_model">
  <pose>1 2 0 0 0 0</pose>
  <link name="box_link">
    <collision name="box_collision">
      <geometry>
        <box>
          <size>1 1 1</size>
        </box>
      </geometry>
    </collision>
    <visual name="box_visual">
      <geometry>
        <box>
          <size>1 1 1</size>
        </box>
      </geometry>
      <material>
        <ambient>0.6 0.6 0.6 1</ambient>
      </material>
    </visual>
  </link>
</model>
```

위 예제에서는 `box_model`이라는 이름의 1x1x1 크기의 상자 모델을 생성하고, `pose`를 통해 초기 위치를 설정하였다. `<collision>`과 `<visual>` 태그는 각각 충돌 및 시각적 요소를 정의한다.

**객체의 물리적 특성**

각 객체에는 중량, 마찰 계수, 밀도와 같은 물리적 특성을 정의할 수 있다. 이를 통해 로봇이 장애물과 상호작용할 때 보다 현실적인 반응을 구현할 수 있다. 각 물리적 특성은 `<inertial>`, `<friction>`, `<dynamics>` 태그를 사용해 정의된다.

```xml
<inertial>
  <mass>2.0</mass>
  <inertia>
    <ixx>0.1</ixx>
    <iyy>0.1</iyy>
    <izz>0.1</izz>
  </inertia>
</inertial>
<surface>
  <friction>
    <ode>
      <mu>0.5</mu>
      <mu2>0.5</mu2>
    </ode>
  </friction>
</surface>
```

* `<mass>`: 물체의 질량을 kg 단위로 지정한다.
* `<inertia>`: 물체의 관성 모멘트를 정의하며, 이는 물체의 회전 저항을 결정한다.
* `<friction>`: 마찰 계수를 설정하여 접촉 면에서 발생하는 마찰력을 조정한다. 이는 로봇이 지면이나 객체와 상호작용할 때의 마찰을 조절하는 데 유용하다.

**모델의 초기 위치 설정 수학적 표현**

모델의 초기 위치와 회전 행렬은 다음과 같이 정의할 수 있다. 여기서 초기 위치는 $\mathbf{P} = \begin{bmatrix} x \ y \ z \end{bmatrix}$, 회전은 각 축에 대한 회전 행렬로 표현할 수 있다.

$$
\mathbf{R}\_{x}(\theta) = \begin{bmatrix} 1 & 0 & 0 \ 0 & \cos \theta & -\sin \theta \ 0 & \sin \theta & \cos \theta \end{bmatrix}
$$

이를 이용해 3축 회전 행렬을 구성하여 초기 회전값을 시뮬레이션 환경에서 반영할 수 있다.

#### 센서 추가하기

로봇 시뮬레이션 환경에서 센서는 중요한 요소이다. 카메라, 라이다(LiDAR), GPS 등의 센서를 시뮬레이션에 추가하여 실제 환경에서의 로봇 인식 능력을 테스트할 수 있다. Gazebo에서는 다양한 센서를 지원하며, 이를 통해 로봇이 환경과 상호작용할 수 있는 방법을 제공한다.

**카메라 센서 예제**

아래는 카메라 센서를 로봇에 장착하는 예제이다. 이 센서는 이미지를 캡처하여 로봇이 주변 환경을 시각적으로 인식할 수 있게 한다.

```xml
<sensor type="camera" name="camera_sensor">
  <pose>0 0 0.5 0 0 0</pose>
  <camera>
    <horizontal_fov>1.047</horizontal_fov>
    <image>
      <width>640</width>
      <height>480</height>
      <format>R8G8B8</format>
    </image>
    <clip>
      <near>0.1</near>
      <far>100</far>
    </clip>
  </camera>
  <update_rate>30</update_rate>
</sensor>
```

* `<pose>`: 센서의 위치를 설정한다.
* `<horizontal_fov>`: 수평 시야각을 정의한다.
* `<clip>`: 이미지의 근거리와 원거리 클리핑을 설정하여 캡처 범위를 제한한다.
* `<update_rate>`: 센서의 업데이트 빈도를 정의하여 주기적으로 데이터를 얻을 수 있다.

**센서 데이터 통합 수학적 표현**

센서 데이터를 로봇의 상태 추정에 통합할 때, 각 센서의 좌표계 변환을 고려해야 한다. 예를 들어, 카메라의 좌표계 $\mathbf{C}$에서 로봇의 좌표계 $\mathbf{B}$로 변환할 때 변환 행렬 $\mathbf{T}\_{\mathbf{C}}^{\mathbf{B}}$를 사용한다. 이는 다음과 같이 표현된다.

$$
\mathbf{T}\_{\mathbf{C}}^{\mathbf{B}} = \begin{bmatrix} \mathbf{R} & \mathbf{t} \ 0 & 1 \end{bmatrix}
$$

여기서 $\mathbf{R}$은 회전 행렬이고 $\mathbf{t}$는 변환 벡터이다. 이를 통해 센서 데이터를 로봇의 중심 좌표계로 변환하여 통합할 수 있다.

#### 시뮬레이션 월드에서 지형 및 표면 특성 설정하기

로봇의 이동 환경을 더욱 정밀하게 구성하기 위해서는 지형과 표면의 물리적 특성을 설정해야 한다. 다양한 표면의 마찰력, 경사도, 장애물 등을 고려하여 실제와 유사한 환경을 조성할 수 있다. 이를 통해 로봇의 주행 성능을 평가하고 환경 적응성을 시험할 수 있다.

**지형 생성 예제**

Gazebo에서는 비정형 지형을 생성하기 위해 `<heightmap>` 태그를 사용할 수 있다. 이를 통해 언덕, 계단, 비포장 도로와 같은 복잡한 지형을 시뮬레이션할 수 있다. `heightmap` 파일은 지형의 높이 정보를 포함하고 있으며, 주로 PNG 또는 RAW 이미지 파일로 표현된다.

```xml
<model name="terrain">
  <pose>0 0 0 0 0 0</pose>
  <link name="terrain_link">
    <collision name="terrain_collision">
      <geometry>
        <heightmap>
          <uri>file://path_to_your_heightmap.png</uri>
          <size>100 100 10</size>
        </heightmap>
      </geometry>
    </collision>
    <visual name="terrain_visual">
      <geometry>
        <heightmap>
          <uri>file://path_to_your_heightmap.png</uri>
          <size>100 100 10</size>
        </heightmap>
      </geometry>
      <material>
        <ambient>0.3 0.2 0.1 1</ambient>
      </material>
    </visual>
  </link>
</model>
```

* `<uri>`: 지형의 높이 정보를 담고 있는 파일의 경로를 지정한다.
* `<size>`: 지형의 전체 크기와 높이 범위를 설정하여 지형의 스케일을 조절할 수 있다.

**마찰 계수 및 표면 특성 설정**

지형의 각 부분에 대한 마찰 계수를 지정하여 로봇의 이동에 대한 난이도를 조절할 수 있다. 이는 `<surface>` 태그를 사용해 정의하며, 로봇의 타이어가 지형과 상호작용할 때의 마찰력을 설정하는데 유용하다.

```xml
<surface>
  <friction>
    <ode>
      <mu>0.8</mu>
      <mu2>0.4</mu2>
    </ode>
  </friction>
</surface>
```

* `<mu>` 및 `<mu2>`: 지형의 주 마찰 계수와 부 마찰 계수를 설정하여 로봇의 미끄러짐 방지를 강화한다. 이 값이 높을수록 표면과의 마찰이 커진다.

**지형 기울기와 로봇 주행 경로 계산**

지형의 기울기와 경사를 고려하여 로봇의 경로를 설정할 수 있다. 지형의 기울기는 경사각으로 표현되며, 로봇의 위치 $\mathbf{P}$에서의 기울기를 계산하는 방법은 다음과 같다.

$$
\text{slope} = \tan^{-1} \left( \frac{\Delta z}{\sqrt{\Delta x^2 + \Delta y^2}} \right)
$$

여기서 $\Delta x$, $\Delta y$, $\Delta z$는 로봇이 이동할 방향에 따른 위치 변화량을 나타낸다. 이를 통해 로봇의 경로 계획 및 주행 안정성을 시뮬레이션할 수 있다.

#### 동적 장애물 추가하기

시뮬레이션 환경에 정적 장애물뿐만 아니라 동적 장애물을 추가하여 더 현실적인 상황을 시뮬레이션할 수 있다. 동적 장애물은 이동 경로를 가지고 있어 로봇의 회피 알고리즘과 반응성을 테스트할 수 있다.

**동적 장애물 설정**

동적 장애물은 `<plugin>` 태그를 이용해 움직임 패턴을 정의하거나, 프로그래밍을 통해 임의의 경로를 설정할 수 있다. 예를 들어, 일정 속도로 직진하는 장애물을 추가할 수 있다.

```xml
<model name="moving_obstacle">
  <pose>5 5 0 0 0 0</pose>
  <link name="moving_link">
    <collision name="collision">
      <geometry>
        <box>
          <size>1 1 1</size>
        </box>
      </geometry>
    </collision>
    <visual name="visual">
      <geometry>
        <box>
          <size>1 1 1</size>
        </box>
      </geometry>
      <material>
        <ambient>0.7 0.2 0.2 1</ambient>
      </material>
    </visual>
  </link>
  <plugin name="moving_plugin" filename="libmoving_plugin.so">
    <velocity>0.5</velocity>
    <direction>1 0 0</direction>
  </plugin>
</model>
```

* `<plugin>`: 동적 장애물의 움직임을 제어하는 플러그인을 로드하여 이동 속도와 방향을 설정한다.
* `<velocity>` 및 `<direction>`: 장애물이 이동할 속도와 방향을 정의하여 지정된 경로를 따라 이동하도록 설정한다.

**동적 장애물과의 충돌 감지**

로봇이 동적 장애물과 충돌할 가능성을 감지하려면 거리 센서나 라이다와 같은 센서를 활용할 수 있다. 센서의 거리 데이터를 활용하여 장애물과의 거리 $\mathbf{d}$를 계산하고, 충돌 가능성을 예측한다. 거리 $\mathbf{d}$는 다음과 같이 정의된다.

$$
\mathbf{d} = \sqrt{(x\_o - x\_r)^2 + (y\_o - y\_r)^2 + (z\_o - z\_r)^2}
$$

여기서 $(x\_o, y\_o, z\_o)$는 장애물의 위치, $(x\_r, y\_r, z\_r)$는 로봇의 위치이다. 이 거리를 기반으로 회피 경로를 설정할 수 있다.

#### 시뮬레이션 환경 내 상호작용 요소 추가하기

로봇이 환경과 다양한 방식으로 상호작용하도록 하기 위해 버튼, 레버, 가변적인 환경 구조물 등의 요소를 시뮬레이션에 포함할 수 있다. 이러한 상호작용 요소는 로봇의 인지 및 제어 알고리즘을 더욱 현실감 있게 테스트하는 데 유용하다.

**버튼 및 트리거 장치 추가**

시뮬레이션 월드에 버튼이나 트리거 장치를 추가하여 로봇이 이를 인식하고 특정 작업을 수행하도록 할 수 있다. 예를 들어, 버튼을 누르면 문이 열리거나 특정 경로가 활성화되는 등 다양한 시나리오를 설정할 수 있다.

```xml
<model name="button_model">
  <pose>3 2 0 0 0 0</pose>
  <link name="button_link">
    <collision name="button_collision">
      <geometry>
        <box>
          <size>0.1 0.1 0.02</size>
        </box>
      </geometry>
    </collision>
    <visual name="button_visual">
      <geometry>
        <box>
          <size>0.1 0.1 0.02</size>
        </box>
      </geometry>
      <material>
        <ambient>0.8 0.1 0.1 1</ambient>
      </material>
    </visual>
  </link>
  <plugin name="button_plugin" filename="libbutton_plugin.so">
    <trigger_event>open_door</trigger_event>
  </plugin>
</model>
```

* `<plugin>`: 버튼이 눌렸을 때 특정 이벤트(예: `open_door`)가 발생하도록 설정한다.
* `<trigger_event>`: 버튼을 통해 트리거할 이벤트 이름을 지정하여, 이후 로봇이 이벤트를 인식하고 반응할 수 있게 한다.

**가변 구조물 추가**

예를 들어, 특정 상황에서 문이 열리고 닫히거나 이동 가능한 벽이 설정되는 시나리오를 구현할 수 있다. 이러한 구조물은 로봇이 지정된 시간이나 이벤트에 따라 환경 변화에 적응하는 능력을 테스트하는 데 사용된다.

```xml
<model name="movable_wall">
  <pose>5 0 0 0 0 0</pose>
  <link name="wall_link">
    <collision name="wall_collision">
      <geometry>
        <box>
          <size>2 0.1 1</size>
        </box>
      </geometry>
    </collision>
    <visual name="wall_visual">
      <geometry>
        <box>
          <size>2 0.1 1</size>
        </box>
      </geometry>
      <material>
        <ambient>0.4 0.4 0.4 1</ambient>
      </material>
    </visual>
  </link>
  <plugin name="movable_wall_plugin" filename="libmovable_wall_plugin.so">
    <open_position>3 0 0</open_position>
    <close_position>5 0 0</close_position>
  </plugin>
</model>
```

* `<open_position>` 및 `<close_position>`: 벽이 열리고 닫히는 위치를 지정하여, 환경의 동적인 변화를 반영한다.
* `<movable_wall_plugin>`: 플러그인을 통해 벽의 이동을 제어하여 로봇의 위치와 상호작용에 따라 벽이 이동하도록 설정한다.

#### 시뮬레이션 환경에서 경로 생성 및 로봇 이동 테스트

시뮬레이션 환경에서 로봇이 특정 경로를 따라 이동하도록 경로를 생성하고 이동을 테스트할 수 있다. 경로 생성과 관련된 다양한 알고리즘을 구현하여 로봇이 목표 지점까지 효율적으로 이동하는지 평가한다.

**경로 생성의 수학적 모델링**

경로 생성 시 경로의 각 지점 $\mathbf{P}\_i$를 기반으로 로봇이 이동할 방향과 거리를 정의한다. 각 지점은 $\mathbf{P}\_i = \begin{bmatrix} x\_i \ y\_i \ z\_i \end{bmatrix}$로 표현되며, 로봇은 연속적인 지점을 따라 이동한다.

각 지점 사이의 거리는 다음과 같이 계산할 수 있다.

$$
d = \sqrt{(x\_{i+1} - x\_i)^2 + (y\_{i+1} - y\_i)^2 + (z\_{i+1} - z\_i)^2}
$$

여기서 $d$는 로봇이 두 지점 사이에서 이동해야 하는 거리이다. 이동 방향 벡터는 다음과 같이 정의된다.

$$
\mathbf{v}*{\text{dir}} = \frac{\mathbf{P}*{i+1} - \mathbf{P}*i}{| \mathbf{P}*{i+1} - \mathbf{P}\_i |}
$$

이를 통해 로봇이 매 지점마다 이동할 방향을 계산하고, 주어진 경로를 따라갈 수 있도록 한다.

**경로 추적을 위한 PID 제어기 적용**

경로를 추적할 때 PID 제어기를 사용하여 목표 경로를 따라가는 로봇의 정확도를 높일 수 있다. PID 제어기는 다음과 같이 표현된다.

$$
u(t) = K\_p e(t) + K\_i \int e(t) , dt + K\_d \frac{de(t)}{dt}
$$

여기서:

* $K\_p$, $K\_i$, $K\_d$: 각각 비례, 적분, 미분 게인으로, 제어기의 튜닝 파라미터이다.
* $e(t)$: 현재 위치와 목표 위치 사이의 오차이다.

PID 제어기를 통해 로봇이 목표 경로를 벗어나지 않도록 오차를 최소화하며 제어할 수 있다. 이를 통해 로봇은 매 지점에서의 오차를 보정하여 경로를 따라가도록 한다.