# 구축 사례: 기초 수준 자율 주행

GNSS를 활용한 기초 수준 자율 주행 시스템은 크게 다음과 같은 절차로 구성된다. 먼저, 단일 혹은 복수 위성 신호를 수신할 수 있는 GNSS 장비를 선정한다. 이후, GNSS 신호를 기반으로 차량의 위치를 계산하고, 측정 오차를 줄이기 위하여 보조 센서(예: IMU, 휠 엔코더)를 결합한다. 마지막으로, 이 정보를 활용해 차량을 자율적으로 제어한다. 여기서는 GNSS 장비 선정부터 실제 차량에 장착하여 기본적인 자율 주행 기능을 구현하기까지의 주요 절차와 수식적 표현을 엄밀히 설명한다.

#### GNSS 수신기 선정

GNSS 수신기 선정은 자율 주행 시스템 구축 시 매우 중요한 문제이다. 초보 단계에서는 다음 조건들을 우선 고려한다.

1. **측위 정확도**: 단일 주파수 L1만 지원해도 괜찮은 수준의 측정값을 얻을 수 있으나, 위치 정밀도(1–2m 정도)를 만족해야 한다.
2. **데이터 출력 속도**: 기본 1 Hz부터 최대 10 Hz 이상을 지원하는 모델이 권장된다.
3. **통신 인터페이스**: 기존 제어기(예: 마이크로컨트롤러, Raspberry Pi 등)와 쉽게 연결 가능한 UART, CAN, I²C, SPI 중 필요에 맞는 인터페이스를 제공해야 한다.

GNSS 모듈에서 수신하는 측정값은 주로 위도(latitude), 경도(longitude), 고도(altitude)이며, 이를 Cartesian 좌표계 혹은 관성 좌표계로 변환하여 제어 시스템에 연동한다. 일반적으로 WGS84 좌표계를 사용하므로, 자율 주행에서 사용하는 지역 좌표계(예: NED, local tangent plane 등)로 투영 변환 과정을 거친다.

#### 기초 측위 알고리즘

GNSS 수신기로부터 획득한 위성 신호를 통해 차량의 위치 $\mathbf{p}\_{\text{GNSS}}$를 산출하는 과정을 간단히 나타내면 아래와 같다.

$$
\mathbf{p}*{\text{GNSS}} =  \begin{bmatrix} \text{Latitude} \ \text{Longitude} \ \text{Altitude} \end{bmatrix} \xrightarrow{\text{좌표변환}} \mathbf{p}*{\text{local}}
$$

여기서 $\mathbf{p}*{\text{GNSS}}$는 WGS84 상의 위치를 나타내고, $\mathbf{p}*{\text{local}}$은 차량이 사용하는 2차원 혹은 3차원 지역 좌표계상의 위치이다. 자율 주행 제어 알고리즘은 대개 $\mathbf{p}\_{\text{local}}$를 사용하므로, 매 주기마다 변환 과정을 수행한다.

실제 구현 시에는 다음과 같은 절차를 따른다.

1. WGS84 좌표계에서 수신한 위도, 경도, 고도 데이터를 ECEF 좌표계(Earth-Centered Earth-Fixed)로 변환한다.
2. ECEF 좌표계를 차량 혹은 로컬 기점에 맞춰 정의한 지역 좌표계(NED 또는 ENU 등)로 투영한다.

이때 사용하는 일반적인 변환식은 다음과 같다.

$$
\mathbf{p}*{\text{ECEF}} = f*{\text{LLA2ECEF}}\bigl(\mathbf{p}*{\text{LLA}}\bigr)
\\
\mathbf{p}*{\text{local}} = f\_{\text{ECEF2local}}\bigl(\mathbf{p}\_{\text{ECEF}}\bigr)
$$

여기서 $f\_{\text{LLA2ECEF}}(\cdot)$, $f\_{\text{ECEF2local}}(\cdot)$은 각각 위도·경도·고도를 ECEF 좌표계로, 다시 지역 좌표계로 변환하는 함수다. 변환 식은 지구의 편평률, 자전축 등을 고려하는 복잡한 계산으로 이루어지지만, 보통 라이브러리나 표준 알고리즘이 잘 정립되어 있어 그대로 사용하면 된다.

***

#### GNSS 데이터의 정밀도 평가

기초 수준 자율 주행에서 가장 문제가 되는 것은 GNSS 측정값의 노이즈와 오차다. 환경 조건(도심 협곡, 터널 등)에 따라 수신 상태가 매우 달라지므로, 단순히 GNSS만으로는 원하는 위치 정확도를 얻기 어려울 때가 많다. 이를 계량적으로 표현하기 위해 GNSS에서 추정되는 오차 공분산 $\mathbf{R}\_{\text{GNSS}}$을 정의한다.

$$
\mathbf{R}\_{\text{GNSS}} =  \begin{bmatrix} \sigma\_x^2 & 0 & 0 \ 0 & \sigma\_y^2 & 0 \ 0 & 0 & \sigma\_z^2 \end{bmatrix}
$$

* $\sigma\_x, \sigma\_y, \sigma\_z$: GNSS로부터 측정되는 위치의 표준편차(보통 수미터 수준)

GNSS 장비가 제공하는 NMEA 데이터 혹은 제조사 SDK를 통해 오차 추정값(HDOP, VDOP 등)을 받아서, 이 행렬을 갱신하는 경우가 많다.

#### 보조 센서(IMU, 휠 엔코더) 융합

기초 수준 자율 주행에서는 GNSS만으로는 순간적인 오차나 위치 끊김 현상을 보완하기 어렵다. 이를 보완하기 위해 IMU(가속도계, 자이로), 휠 엔코더를 통해 차량의 속도, 회전 각속도 등을 측정하고, GNSS와 융합(Fusion)하여 더 정확한 위치와 자세를 추정한다.

자율 주행에서 흔히 사용하는 센서 융합 알고리즘으로 확장 칼만 필터(EKF)가 있다. 상태 벡터 $\mathbf{x}\_k$를 다음과 같이 정의한다고 하자.

$$
\mathbf{x}\_k =  \begin{bmatrix} x\_k \  y\_k \  \theta\_k \  v\_k \  \omega\_k \end{bmatrix}
$$

* $x\_k$, $y\_k$: 차량의 2차원 위치
* $\theta\_k$: 차량의 방향(heading)
* $v\_k$: 차량의 전진 속도
* $\omega\_k$: 차량의 회전 각속도

센서 융합 과정은 다음과 같이 상태 예측(prediction)과 업데이트(update) 단계를 반복한다.

1. **예측 단계**: IMU와 휠 엔코더 데이터를 통해 차량이 이동한 거리를 추정하고, $k$ 시점에서 $k+1$ 시점의 상태를 추정한다.
2. **업데이트 단계**: GNSS 측정값(즉, $x$, $y$ 위치 측정)이 들어오면, 측정값과 예측값의 차이를 이용해 상태 벡터를 보정한다.

이를 확장 칼만 필터의 일반화된 상태 방정식으로 나타내면:

$$
\mathbf{x}\_{k+1} = \mathbf{f}\bigl(\mathbf{x}*k, \mathbf{u}*k\bigr) + \mathbf{w}*kzk+1=h(xk+1)+vk+1\mathbf{z}*{k+1} = \mathbf{h}\bigl(\mathbf{x}*{k+1}\bigr) + \mathbf{v}*{k+1}
$$

* $\mathbf{f}(\cdot)$: 차량의 운동 방정식을 나타내는 비선형 함수
* $\mathbf{u}\_k$: 휠 엔코더, IMU 정보에 해당하는 제어 혹은 추정 입력
* $\mathbf{z}\_{k+1}$: GNSS로부터 측정되는 위치 관측치
* $\mathbf{h}(\cdot)$: 실제 측정 모델
* $\mathbf{w}\*k, \mathbf{v}\_{k+1}$: 각각 시스템 노이즈, 측정 노이즈

이 과정을 통해 GNSS 신호가 일시적으로 끊기거나 점프하는 상황에서도, IMU와 휠 엔코더로부터 예측된 위치를 이용해 어느 정도 안정적인 추정이 가능하다.

#### 구현 예시 코드

아래는 ROS(Robot Operating System) 환경에서 GNSS, IMU, 휠 엔코더 데이터를 구독(subscribe)하고, 확장 칼만 필터를 통해 위치를 추정하는 예시(간단화)다.

```python
import rospy
from sensor_msgs.msg import Imu
from nav_msgs.msg import Odometry
from some_gnss_pkg.msg import GnssData

# EKF 상태 벡터, 공분산 등 초기화
ekf_state = [0.0, 0.0, 0.0, 0.0, 0.0]  # x, y, theta, v, omega

def gnss_callback(data):
    # data.latitude, data.longitude
    # 1) 위경도 -> 지역 좌표계 변환
    # 2) EKF update 단계
    pass

def imu_callback(data):
    # data.linear_acceleration, data.angular_velocity
    # EKF prediction 단계에서 사용
    pass

def encoder_callback(data):
    # data.pose.pose.position, data.pose.pose.orientation
    # EKF prediction 단계에서 사용
    pass

def main():
    rospy.init_node('gnss_imu_ekf_node')
    rospy.Subscriber('/gnss', GnssData, gnss_callback)
    rospy.Subscriber('/imu', Imu, imu_callback)
    rospy.Subscriber('/encoder', Odometry, encoder_callback)
    rospy.spin()

if __name__ == "__main__":
    main()
```

실제 구현 시에는 각 센서로부터 들어오는 위치, 가속도, 각속도 정보를 적절히 EKF 상태 벡터와 공분산 행렬에 반영해야 하며, 타이밍 동기화 문제(Timestamp sync)도 신경써야 한다.

#### 시스템 아키텍처 예시

아래는 mermaid로 표현한 간단한 시스템 아키텍처 예시다.

{% @mermaid/diagram content="flowchart LR
A\["GNSS Receiver"] --> B\["Localization Module (EKF)"]
C\["IMU Sensor"] --> B
D\["Wheel Encoder"] --> B
B --> E\["Vehicle Control"]" %}

EKF 기반의 Localization Module에서 GNSS 수신값, IMU, 휠 엔코더 정보를 종합하여 실시간으로 차량 위치와 자세를 추정하고, Vehicle Control 모듈에서는 이 정보를 바탕으로 주행 경로를 제어한다.

#### 하드웨어 설치와 배선

기초 수준 자율 주행을 실현하기 위해서는 GNSS 수신기 및 보조 센서(IMU, 휠 엔코더 등)를 차량에 물리적으로 장착하고, 데이터를 송수신할 수 있도록 배선을 구성해야 한다. 일반적으로 다음과 같은 단계를 거친다.

1. **GNSS 안테나 설치 위치 선정**
   * 차량 지붕 등 전파가 잘 들어오는 곳을 우선적으로 고려한다.
   * 가능한 차량 중심축 근처에 설치하되, 지붕의 중앙 부분이 가장 흔히 사용된다.
2. **IMU 장착을 위한 기준축 정렬**
   * IMU 센서가 측정하는 가속도 및 각속도 축을 차량의 전진 방향, 좌우 방향, 수직 방향과 정확히 일치시키도록 노력한다.
   * 보정 혹은 보정 행렬을 적용해 차량 기준축과 IMU 기준축 사이의 자세 편차(roll, pitch, yaw)를 최소화한다.
3. **휠 엔코더 신호 케이블 연결**
   * 휠 엔코더는 보통 차량의 구동축 또는 전륜, 후륜에 부착된 엔코더 센서와 연결된다.
   * 엔코더 펄스를 제어기나 EKF 알고리즘을 수행하는 CPU로 전달하기 위한 디지털 신호 라인을 구성한다.
4. **전원 및 통신 배선**
   * GNSS 모듈, IMU 센서, 엔코더 등의 전원 요구 사항(5 V, 12 V 등)을 파악하고, 차량 전원(배터리) 또는 별도의 전원공급장치를 이용해 안정적으로 전압을 공급한다.
   * 통신 프로토콜(UART, CAN, 이더넷 등)에 맞춰 제어기(예: 차량용 PC, 마이크로컨트롤러, SBC 등)와 센서를 연결한다.

일련의 과정을 거쳐 물리적 구성과 배선을 완료하면, 제어 시스템(ROS, 임베디드 코딩 등)에서 각 센서가 출력하는 데이터를 실시간으로 수신할 수 있다. 이때 센서별 데이터 레이트를 고려하여 처리 속도가 충분한지, 노이즈 필터링이 필요한지 등의 점검이 필수적이다.

#### 로컬맵 기반 항법

기초 수준 자율 주행에서 GNSS로부터 획득한 위치만을 이용해 차량을 주행시키려면, 간단한 로컬맵 혹은 Waypoint 기반의 경로 설정이 필요하다. 보통 다음과 같은 방식으로 구현할 수 있다.

1. **Waypoints 설정**
   * 경로를 따라 일정 간격(예: 1–5 m)으로 Waypoint를 설정한다. 각 Waypoint는 $\[x\_i, y\_i]$로 표현된다.
   * GNSS를 통해 지역 좌표계로 변환한 뒤, Waypoint 리스트를 차량 내부에 저장한다.
2. **주행 중 Waypoint 추종**
   * 차량의 현재 위치 $\mathbf{p}*{\text{local}} = \[x, y]^\top$와 Waypoint $\mathbf{p}*{i} = \[x\_i, y\_i]^\top$ 간의 오차를 계산한다.
   * 오차 $\Delta \mathbf{p} = \[x\_i - x,, y\_i - y]^\top$를 최소화하도록 조향각(steering angle)과 속도를 제어한다.
3. **Heading 제어**
   * 차량의 현재 방향 $\theta$와 Waypoint로 향하는 방향 $\theta\_i = \arctan2(y\_i - y,; x\_i - x)$의 차이를 구한다. 이를 $\Delta \theta = \theta\_i - \theta$라 하면, 간단한 P 제어나 PD 제어를 통해 조향각을 결정한다.
   * 예: 조향각 명령 $\delta$를 $\delta = k\_p \Delta \theta + k\_d \frac{d(\Delta \theta)}{dt}$ 와 같이 설정할 수 있다.
4. **속도 제어**
   * Waypoint 간 거리에 따라 목표 속도 $v\_{\text{ref}}$를 달리 설정하거나, 곡선 구간에서 감속하는 규칙을 적용한다.
   * 전진 속도 명령 $v$를 $v = \max(0,; v\_{\text{ref}} - k\_v |\Delta \mathbf{p}|)$ 와 같이 설정하여, Waypoint 근처에서는 속도를 줄이도록 조정할 수도 있다.

일반적으로 이 방식은 전방 주시형 제어(Look-ahead control) 혹은 순수 추종(Pure pursuit) 알고리즘으로 확장된다.

***

#### 전방 주시형 제어(Pure Pursuit) 알고리즘 개요

로컬맵 기반 항법의 대표적인 방식으로 전방 주시형 제어(Pure Pursuit)가 있다. 기본 아이디어는 차량 앞으로 일정 거리(look-ahead distance) $L$만큼 떨어진 지점에서, 곡률을 결정하는 것이다.

1. **Look-ahead distance 설정**
   * 차량 속도에 비례하여 $L$을 동적으로 조절할 수 있다. 예: $L = L\_0 + k\_L \cdot v$, 여기서 $L\_0$는 최소값, $k\_L$은 계수, $v$는 차량 속도.
2. **목표점(Goal point) 찾기**
   * 로컬맵의 Waypoint 중, 현재 차량 위치로부터 거리 $L$에 가장 가까운 점을 목표점으로 설정한다.
3. **조향각 계산**
   * 차량을 목표점으로 향하게 하는 곡률 $\kappa$를 구하고, 조향각 $\delta$와 매핑한다.
   * 일반적인 수식은 다음과 같다.

     $$
     \kappa = \frac{2 \sin \alpha}{L}
     $$

     * $\alpha$: 차량 중심축과 목표점 방향을 잇는 선분이 이루는 각도
     * $L$: look-ahead distance
   * 차량 조향 메커니즘(자전거 모델 가정)을 고려하면,

     $$
     \delta = \arctan\bigl(L\_r \cdot \kappa\bigr)
     $$

     * $L\_r$: 차량의 wheelbase(앞바퀴와 뒷바퀴 축간 거리)

이를 통해 GNSS와 센서 융합으로부터 위치를 얻은 후, 로컬맵 상 Waypoint를 추적하면서 자율 주행을 구현할 수 있다.

***

#### 단일 및 복수 GNSS 사용 시 고려사항

자율 주행을 구현할 때, 단일 GNSS 모듈만 사용하는 경우와 복수 GNSS 모듈 혹은 RTK(Real-Time Kinematic) 수신기를 사용하는 경우에 따라 상황이 달라진다.

1. **단일 GNSS**
   * 단순 설치 및 비용이 저렴하나, 측정 오차(수미터 수준)가 상대적으로 크다.
   * 보조 센서(IMU, 휠 엔코더) 융합으로 어느 정도 오차 보완이 가능하나, 도심처럼 주변 환경이 복잡한 곳에서는 정확도가 크게 떨어질 수 있다.
2. **복수 GNSS**
   * 차량 여러 위치(예: 앞뒤, 양쪽 등)에 GNSS 안테나를 설치하여 다중 신호를 수신한다.
   * RTK 기반 GNSS 수신기를 이용하면 수 센티미터급 측위 정밀도를 달성할 수 있지만, 기반국(base station) 설치 또는 네트워크 RTK 서비스 등이 필요하다.
3. **GNSS+센서 융합 전략**
   * IMU, 휠 엔코더는 차량의 상대 이동량을 정밀하게 추정 가능하므로, GNSS 위치의 절대 오차와 센서로부터 추정되는 상대 오차를 상호 보완한다.
   * 오차 공분산을 실시간으로 추정하면서, GNSS 신호가 불안정할 때에는 IMU/엔코더 의존도를 높이고, GNSS 신호가 안정적일 때에는 GNSS의 가중치를 높이는 전략을 취한다.

#### RTK GNSS 예시

만약 기초 수준 자율 주행에서도 높은 측정 정확도가 필요한 경우(예: 도로 차선 맞춤 주행, 정밀 주차 등), RTK GNSS를 도입할 수 있다. RTK GNSS는 기존 GNSS 신호에 보정 정보를 적용하여 위치 정확도를 cm 단위까지 끌어올린다.

1. **기반국 설치**
   * 현장에 기준 위치가 정확히 알려진 GNSS 수신기를 배치하고, 이 수신기에서 전송되는 보정 데이터를 이동국(차량에 장착된 GNSS 수신기)이 수신한다.
   * 혹은 상업용 네트워크 RTK 서비스(예: VRS, NTRIP 등)를 사용한다.
2. **Carrier-phase 측정**
   * GNSS 위성 신호의 반송파 위상 차이를 정밀하게 측정하여, 의사거리(Pseudo-range) 기반 측위 대비 훨씬 높은 정확도를 확보한다.
3. **융합 알고리즘 변경**
   * RTK 기반 위치 측정은 오차 공분산이 훨씬 작아지므로, $\mathbf{R}\_{\text{GNSS}}$ 값을 작게 설정한다.
   * EKF 등을 적용할 때, GNSS 측정값의 신뢰도가 높아져 IMU, 엔코더가 제공하는 상대 위치 추정 정보와 균형을 맞춰 융합 가능하다.

#### 간단 필드 테스트 절차

GNSS 센서와 IMU, 휠 엔코더를 모두 장착하고, 소프트웨어(ROS 혹은 기타 제어 환경)에서 EKF를 구성했다면, 필드 테스트 절차는 대략 다음과 같다.

1. **초기화**
   * 차량을 움직이지 않은 상태에서 GNSS 위치, IMU, 엔코더 데이터를 일정 시간 수집한다. 이 데이터를 바탕으로 초기 오프셋(가속도 바이어스, 자이로 바이어스 등)을 추정한다.
2. **GNSS Lock 확인**
   * GNSS 수신기가 충분히 많은 위성(4개 이상)을 추적하고, DOP 값(HDOP, PDOP 등)이 일정 수준 이하인지(예: 2.0 미만)를 확인한다.
3. **저속 주행**
   * 저속(시속 5–10 km/h)으로 직선 구간을 주행하면서, 실시간 측정 위치와 실제 도로 지도를 비교한다.
   * IMU, 엔코더, GNSS 융합값이 안정적인지 모니터링하고, 초기 튜닝 파라미터(EKF 잡음 공분산 등)를 조정한다.
4. **곡선 구간 주행**
   * 곡선 도로나 U턴 구간을 주행하면서, 조향각/회전 각속도에 대해 EKF가 정상적으로 동작하는지 확인한다.
   * GNSS 신호가 일시적으로 가려지는 상황(건물, 교각 아래 등)에서 IMU와 엔코더만으로 위치 추정이 어느 정도 정확하게 이어지는지 확인한다.
5. **로그 데이터 분석**
   * 주행이 끝난 후, ROS bag 파일 혹은 별도 로거에 저장된 센서 데이터를 분석한다.
   * GNSS 단독 위치와 EKF 융합 위치를 비교하여 오차 통계를 계산하고, 필요하면 추가적인 파라미터 조정을 진행한다.

#### 보정 및 캘리브레이션

차량이 움직임에 따라 IMU에는 편향(bias), 스케일 팩터(scale factor) 등의 오차가 발생할 수 있다. GNSS와 비교하여 다음을 보정한다.

1. **IMU 바이어스 추정**
   * 차량이 정지해 있을 때 측정되는 가속도, 각속도의 평균값을 측정해 바이어스를 추정한다.
   * EKF나 전용 캘리브레이션 알고리즘을 통해 주행 중에도 바이어스가 점진적으로 업데이트되도록 설정한다.
2. **엔코더 스케일 팩터 보정**
   * 바퀴의 지름, 기어비, 엔코더 펄스 당 도수(degree), 바퀴 슬립 등을 정확히 파악하지 못하면 속도 및 이동 거리 측정에 오차가 생긴다.
   * 직선 구간에서 실제 주행 거리와 엔코더로 측정된 거리를 비교하여 스케일 팩터를 보정한다.
3. **GNSS 안테나 오프셋 보정**
   * GNSS 안테나가 차량 무게중심과 떨어져 있는 경우, 회전 시 위치 측정에 편차가 생길 수 있다.
   * 차량 기준 좌표계에서 GNSS 안테나 좌표계를 정의하고, $\[x\_{\text{ant}},; y\_{\text{ant}},; z\_{\text{ant}}]$ 오프셋을 EKF에 반영한다.

#### 차량 동역학 모델(자전거 모델)

기초 수준 자율 주행을 구현할 때, 차량의 움직임을 단순화하여 모델링하는 것이 일반적이다. 대표적으로 \*\*자전거 모델(bicycle model)\*\*을 사용한다. 이 모델에서는 앞바퀴와 뒷바퀴가 각각 하나의 바퀴로 단순화되어, 회전이나 주행을 해석하기 쉽다. 2차원 평면 상에서, 차량의 위치와 자세를 다음 상태로 정의할 수 있다.

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

* $x(t), y(t)$: 차량 무게중심(혹은 뒷바퀴 중심)의 위치
* $\theta(t)$: 차량의 방향(heading)

자전거 모델에서 차량의 속도 $v$와 조향각 $\delta$를 입력이라 할 때, 운동방정식은 다음과 같이 표현된다.

$$
\dot{x}(t) = v(t)\cos\theta(t)
\\
\dot{y}(t) = v(t)\sin\theta(t)
\\
\dot{\theta}(t) = \frac{v(t)}{L\_r} \tan \delta(t)
$$

* $L\_r$: 뒷바퀴에서 앞바퀴까지의 축간 거리(wheelbase)
* $\delta(t)$: 차량 앞바퀴 조향각

간단화된 형태이므로, 실제 차량의 롤/피치, 에어로다이내믹, 코너링 시 타이어 슬립 등은 반영하지 않는다. 그럼에도 불구하고 저속 또는 중속 주행에서는 이 자전거 모델이 상당히 유용하다.

#### Stanley 제어기 개요

기초 수준 자율 주행에서 경로 추종(path tracking)을 위해 널리 쓰이는 방법 중 하나가 \*\*Stanley 제어기(Stanley Controller)\*\*이다. Pure Pursuit에 비해 조향각 계산 과정이 직관적이며, 주행 경로에 접근할 때의 측방 편차(lateral error)를 사용한다.

1. **측방 편차 eye\_y 정의**
   * 차량의 무게중심에서 목표 경로(주행할 레퍼런스 선)까지의 측방 거리.
   * 경로가 주어지면, 해당 경로의 최근접점을 찾아 측방 거리를 정의한다.
2. **제어 법칙**

   * Stanley 제어기의 기본 제어각 $\delta$는 다음과 같이 정의된다.

   $$
   \delta = \theta\_e + \arctan\left(k \cdot \frac{e\_y}{v}\right)
   $$

   * $\theta\_e$: 차량의 방향 θ\theta와 경로 법선 벡터가 이루는 각도 오차
   * $k$: Stanley 이득 계수(gain)
   * $e\_y$: 측방 편차
   * $v$: 차량 속도
3. **해석**
   * $\theta\_e$가 크면, 차량의 진행 방향이 경로와 크게 어긋난 상태이므로 직접적인 조향 각도를 크게 만들어 빠르게 복귀하려 한다.
   * 측방 편차 $e\_y$가 커질수록, $\delta$가 커져서 차량이 경로 쪽으로 적극적으로 접근한다. 단, 차량 속도 $v$가 커지면 너무 과도한 조향으로 인해 불안정해질 수 있으므로, $\frac{e\_y}{v}$ 항이 그 영향도를 줄인다.

#### 자율 주행 소프트웨어 구조(Stanley 예시)

Stanley 제어기를 적용한 자율 주행 소프트웨어 구조는 대략 아래와 같이 구성할 수 있다.

{% @mermaid/diagram content="flowchart LR
A\["Localization (GNSS+IMU+Encoders)"] --> B\["Path Planning / Waypoint Generation"]
B --> C\["Stanley Controller"]
C --> D\["Vehicle Actuator (Steering, Throttle, Brake)"]" %}

1. **Localization**
   * GNSS, IMU, 휠 엔코더를 EKF 등으로 융합하여 차량 현재 위치 plocal\mathbf{p}\_{\text{local}}과 방향 θ\theta를 실시간 추정한다.
2. **Path Planning / Waypoint Generation**
   * 로컬맵 또는 사전에 정의된 Waypoint 목록을 기반으로, 목표 경로(레퍼런스 선)를 구성한다.
   * 차량 주변 장애물이 적은 단순 상황에서는 미리 정해진 경로를 따라주기만 해도 충분하다.
3. **Stanley Controller**
   * 위에서 추정된 차량의 위치와 방향, 목표 경로의 측방 편차 $e\_y$, 방향 오차 $\theta\_e$를 계산한다.
   * 제어각 δ\delta와 속도 명령 $v$를 결정하여 차량 구동계와 조향계에 전달한다.

#### 간단 Stanley 제어 코드 예시

아래 Python 코드는 Stanley 제어를 간단히 보여주는 예시다.

```python
import math

def stanley_control(x, y, theta, v, waypoints, k):
    """
    x, y: 현재 차량 위치
    theta: 현재 차량 방향(heading, rad)
    v: 차량 속도(m/s)
    waypoints: (x_ref, y_ref) 리스트
    k: Stanley gain
    """
    # 1) 현재 위치에서 가장 가까운 waypoints 찾기
    #    (실제로는 KD-Tree 또는 최근접 탐색 알고리즘 사용)
    min_dist = float('inf')
    closest_wp = None
    for (wx, wy) in waypoints:
        dist = (wx - x)**2 + (wy - y)**2
        if dist < min_dist:
            min_dist = dist
            closest_wp = (wx, wy)

    # 2) 측방 편차 e_y 계산
    #    간단히 waypoints 간 방정식으로 경로 각도 추정
    #    여기서는 단순 예시로 최근 waypoint만 사용
    path_angle = math.atan2(closest_wp[1] - y, closest_wp[0] - x)
    theta_e = path_angle - theta

    # 측방 편차(직선 경로 가정 시)
    e_y = math.sqrt(min_dist)

    # 3) Stanley 제어각 계산
    delta = theta_e + math.atan2(k * e_y, v + 1e-6)

    return delta
```

* 실제로는 Waypoint 간의 연결을 기반으로 경로 각도를 계산하고, $e\_y$ 부호(좌측 편차인지 우측 편차인지)도 정확히 판단해야 한다.
* 속도 $v$가 매우 작으면 $\frac{e\_y}{v}$가 커져 제어각이 매우 커질 수 있으므로, 코드에서 `v + 1e-6`처럼 분모가 0이 되지 않도록 처리한다.

#### 안전 기능과 Fail-safe 설계

기초 수준 자율 주행이라 하더라도, 안전 기능을 반드시 고려해야 한다. 다음 사항들을 시스템 설계에 반영한다.

1. **수동 모드(MANUAL)와 자동 모드(AUTO) 전환**
   * 운전자의 스위치 조작 혹은 UI 인터페이스를 통해 언제든지 수동 모드로 전환할 수 있어야 한다.
   * 자동 주행 중 장애물을 감지하거나 GNSS 신호가 상실되는 등 이상 상황이 발생하면 자동으로 수동 모드로 돌아간다.
2. **E-Stop(긴급 정지)**
   * 차량을 즉시 정지시킬 수 있는 하드웨어적 스위치(무선/유선)를 구비한다.
   * 제어기(소프트웨어)에서 이상값이 감지될 경우에도 브레이크를 걸어 정지시키는 로직을 두는 것이 일반적이다.
3. **GNSS 데이터 검증**
   * GNSS 위치값이 갑자기 큰 변동을 보일 경우(예: 건물에 의해 반사된 멀티패스, 신호 점프 등), 신뢰도를 낮춰 EKF 측정 업데이트를 일시적으로 보류하거나 무게를 줄인다.
   * 일정 시간 이상 GNSS 신호가 들어오지 않으면, IMU/엔코더 기반 사사오입(dead reckoning)을 진행하면서도 주행 속도를 제한하는 등 보수적인 전략을 택한다.
4. **IMU/엔코더 이상 모니터링**
   * 엔코더 펄스가 갑자기 0이 되거나, IMU 각속도가 불합리하게 큰 값을 보고하면 센서 고장으로 판단하고, 자율 모드를 해제한다.

#### 차량 속도 제어(PID 예시)

조향 외에도 차량의 속도를 일정하게 유지하거나 가감속을 제어해야 한다. 간단한 방법으로는 **PID 제어기**를 사용할 수 있다. 목표 속도 vrefv\_{\text{ref}}와 측정 속도 vv가 주어졌을 때, 제어 입력(스로틀, 모터 토크 등) uu는 다음과 같이 표현된다.

$$
\begin{aligned} e\_v(t) &= v\_{\text{ref}}(t) - v(t), \ u(t) &= K\_p e\_v(t) + K\_i \int\_0^t e\_v(\tau), d\tau + K\_d \frac{d,e\_v(t)}{dt}. \end{aligned}
$$

* $e\_v(t)$: 속도 오차
* $K\_p, K\_i, K\_d$: 비례, 적분, 미분 이득

실차에서는 가속 페달, 모터 제어, 브레이크 등을 별도의 로직으로 구분하기도 하며, IMU에서 측정한 종방향 가속도 정보 등을 이용해 더 정교한 속도 제어를 수행할 수도 있다.

#### 실시간 성능 및 지연 고려

자율 주행 시스템에서는 센서 데이터가 지연되거나, 네트워크 대역폭이 부족해지는 상황을 방지해야 한다.

1. **센서 데이터 타임스탬프**
   * GNSS, IMU 등에서 각각 다른 시점에 발생하는 데이터를 시간축으로 정렬(synchronization)하거나 보간(interpolation) 작업이 필요하다.
   * ROS 등에서는 메시지에 타임스탬프가 포함되므로, EKF나 제어 알고리즘에서도 해당 타임스탬프를 사용해 처리한다.
2. **제어 주기 결정**
   * 기본적으로 50 Hz(20 ms) 또는 100 Hz(10 ms) 단위로 EKF 업데이트와 제어 명령을 산출하면, 일반적인 저속 자율 주행에는 충분하다.
   * IMU 데이터가 200 Hz~~1000 Hz 이상 들어오는 경우, EKF 내부에서 추가적인 중간 예측을 수행하고, GNSS 데이터는 5 Hz~~10 Hz로 들어와도 EKF 업데이트를 할 때만 반영한다.
3. **연산 지연(latency) 측정**
   * 센서에서 측정 -> 통신 -> EKF 연산 -> 제어 명령 생성 -> 구동계 반응 에 이르는 총 소요 시간을 측정하고, 이를 모델링하거나 보상(anti-latency 기법)을 적용하기도 한다.

#### 추가 고려사항: 로컬 맵 생성 및 경로 계획

기초 수준 자율 주행이라도, 주변 환경을 어느 정도 인지하고, 차량이 주행할 수 있는 공간을 파악하는 과정(맵 생성 및 경로 계획)이 필요하다. 간단하게는 사전에 준비된 정적 맵 위에 Waypoint를 배치하거나, 직접 주행하면서 얻은 센서 정보를 기반으로 로컬 맵을 형성하는 방식을 고려할 수 있다.

1. **간단 로컬맵 (Static Waypoint)**
   * 테스트 코스나 특정 도로 구간만 운행할 때, 미리 획득한 GNSS 좌표(여러 지점)를 연결해 Waypoint 세트를 만든다.
   * 차량은 이 Waypoint 열을 순차적으로 추종하므로, 장애물이 없거나 무시해도 되는 환경(실험장, 폐쇄된 도로)에서 유효하다.
2. **동적 로컬맵 (센서 융합)**
   * 라이다(LiDAR), 초음파 센서, 카메라 등으로 주변 장애물을 인식하고, 접근 가능 영역(drivable area)을 실시간 생성한다.
   * GNSS 기반 절대 위치와 IMU, 휠 엔코더 기반 상대 이동 정보를 통해, 차량 주변의 지도를 업데이트한다.
3. **경로 계획(Path Planning)**
   * 기초 수준에서는 Waypoint 리스트를 따라가는 방식이 일반적이지만, 장애물이 있을 경우 간단한 회피 경로를 만들어야 한다.
   * 예: $A^\star$ 알고리즘, RRT, Dijkstra 등 그래프 탐색 기법을 사용하여 로컬 맵 상에서 시작점에서 목표점까지의 경로를 찾는다.
   * 자율 주행 알고리즘은 일정 주기로 경로를 재계산하며, 실제 주행과 오차가 커지거나 장애물이 발생할 때 유연하게 대응할 수 있다.

#### 장애물 감지 및 회피

기초 수준 자율 주행에선 GNSS, IMU, 휠 엔코더만으로는 장애물 정보를 얻을 수 없다. 따라서 간단한 센서를 추가로 탑재하거나, 사람이 미리 도로를 확인하고 장애물 없는 환경을 구성한다. 만약 최소한의 장애물 감지를 하려면 다음과 같은 방법을 고려할 수 있다.

1. **초음파 센서(울트라소닉) 활용**
   * 차량 전방 및 후방에 설치하여 근거리(수십 cm\~수 m) 충돌을 감지한다.
   * 저속에서 주차, 서행할 때 충돌 방지에 기여한다.
2. **레이더(RADAR) 센서**
   * 중장거리(수 m\~수십 m)의 장애물 유무와 상대 속도를 감지할 수 있어, 일반 주행 상황에서도 활용 가능하다.
   * 비교적 비용이 저렴하고 날씨(비, 안개) 영향이 적은 편이다.
3. **간단 카메라/비전 센서**
   * 전방 영상 분석을 통해 차선, 물체 등을 식별할 수 있으나, 딥러닝 기반 물체 감지가 필요하면 컴퓨팅 성능을 고려해야 한다.
   * 기초 수준에서는 특정 색상(콘, 표지판 등)만 인식하는 룰 기반 알고리즘으로 시작하기도 한다.

이러한 센서 정보를 가져오면, EKF에서 추정된 차량 위치와 결합하여 장애물 상대 위치를 추정하고, 간단한 회피 경로(steering angle 조정, 감속 등)를 생성할 수 있다.

#### 시뮬레이션 환경 구축

실차 테스트 전, 시뮬레이션 환경에서 GNSS, IMU, 휠 엔코더, 제어 알고리즘을 모두 가상으로 검증해볼 수 있다. 대표적인 오픈소스/상용 시뮬레이터를 활용하거나, 간단한 2D/3D 물리 엔진을 통해 가상 차량 모델을 구동한다.

1. **ROS + Gazebo**
   * ROS 환경에서 Gazebo 시뮬레이터를 구동하면, 다양한 센서(카메라, LiDAR, IMU 등)를 가상으로 장착할 수 있다.
   * GNSS 플러그인을 사용하면, 차량이 달리는 맵 상에서 위도·경도 정보를 모사해준다.
2. **LGSVL 시뮬레이터**
   * 자율 주행 플랫폼인 Apollo, Autoware 등과 연동할 수 있는 시뮬레이터로, 실제 도로 환경(신호등, 차선, 차량 교통 등)을 꽤 현실감 있게 재현한다.
   * GNSS 오류나 노이즈도 일정 수준으로 모사 가능하다.
3. **Unreal Engine 기반 시뮬레이터 (CARLA 등)**
   * CARLA 시뮬레이터도 자율 주행 연구용으로 많이 쓰이며, GNSS, IMU, 카메라 등 주요 센서를 가상화한다.
   * Python API를 통해 주행 제어, 센서 데이터 수집 등이 가능하여, 기초 수준 자율 주행 알고리즘 검증에 적합하다.

시뮬레이션에서 충분히 제어 파라미터와 센서 융합 알고리즘을 검증한 후, 실차에 적용하면 시행착오를 크게 줄일 수 있다.

#### 주행 로그 데이터 분석

테스트 주행 후, 로그 데이터를 체계적으로 수집·분석하면 다음 단계 개선에 큰 도움이 된다.

1. **로그 구성**
   * GNSS: 시간별 위치(위도·경도), HDOP, 위성 수 등
   * IMU: 선형 가속도, 각속도, 자이로 바이어스 추정값 등
   * 엔코더: 차량 속도, 주행 거리, 바퀴별 회전수 등
   * 제어 명령: 조향각, 목표 속도, 실제 속도 등
2. **오프라인 재생(Replay)**
   * ROS의 경우, rosbag 파일을 재생하여 알고리즘을 반복 실행해볼 수 있다. 파라미터를 변경해가며 오프라인에서 성능을 평가한다.
   * GNSS 수신 불량 시점, IMU 드리프트 시점 등을 정확히 찾아내어, EKF 공분산 조정 등 개선점을 도출한다.
3. **오차 통계**
   * GNSS 단독 위치 vs. EKF 융합 위치 vs. 실제 기준 값(중앙선, 지상 기준점 등)을 비교한다.
   * 루트제곱평균오차(RMSE), 최대 오차(max error), 95% 신뢰구간 등에 대한 수치를 계산하여, 시스템 성능을 정량적으로 평가한다.

#### 운전자 개입 및 안전 조치

기초 수준 자율 주행에서 사람이 완전히 운전대를 놓아서는 안 된다. 시스템이 불안정하거나 센서 고장, 알고리즘 오작동이 발생할 수 있기 때문이다. 다음과 같은 인간-시스템 인터페이스(HMI)를 준비한다.

1. **Driver Override**
   * 수동 조향이나 브레이크, 가속 페달을 조작하면, 자동 제어가 즉시 해제되고 운전자 의도가 우선시된다.
   * 전기 신호로 우선순위를 설정하거나, 물리적 릴레이(steering column)에 의해 결정될 수 있다.
2. **경고 알람**
   * GNSS 신뢰도 저하, IMU 이상, 엔코더 끊김 등 특정 오류가 발생할 때 운전자에게 시청각 경고를 제공한다.
   * 시스템이 자동으로 비상 정지 또는 서행 모드로 전환되도록 하여 사고 위험을 줄인다.
3. **페일세이프(Fail-safe) 상태**
   * 전력 계통 문제나 제어기 오류 발생 시, 차량이 기본적으로 정지하거나 안전 모드로 전환되도록 설계한다.
   * 브레이크 라인이나 조향 라인이 전자 제어 회로와 독립적인 매커니즘(유압, 기계식)을 유지하는 것이 바람직하다.

#### 향후 확장

기초 수준에서 GNSS+IMU+휠 엔코더로 시작한 자율 주행은, 요구되는 정확도와 환경 복잡도에 따라 다음 단계를 고려할 수 있다.

* **RTK GNSS, 다중 안테나**: 센티미터급 측위 및 자세 추정(heading, roll, pitch)
* **LiDAR, 카메라, 라이다 SLAM**: 도심 환경에서의 정밀 맵 구축 및 장애물 인식
* **V2X 통신**: 교통 신호, 다른 차량과 정보 교환을 통한 안전도 향상
* **고급 제어 알고리즘**: Model Predictive Control(MPC), Adaptive Control, AI 기반 제어

이처럼 점진적 확장을 위해서는 테스트와 안전 절차를 반복하며 시스템 성능을 끌어올린다.

#### 관련 표준 및 참고 자료

기초 수준 자율 주행 시스템에서 GNSS를 비롯한 센서 선택, 안전 기능, 소프트웨어 아키텍처를 설계할 때 다음과 같은 표준·규격, 참고 자료를 확인할 수 있다.

1. **NMEA 0183 / NMEA 2000**
   * GNSS 수신기가 출력하는 데이터 형식을 정의하는 국제 표준.
   * 위치, 속도, 시간 등 정보를 일관된 포맷으로 제공하므로, 수신기 제조사별로 큰 차이 없이 처리 가능하다.
2. **ISO 26262 (자동차 기능 안전)**
   * 자동차 전장품의 기능 안전(Functional Safety)을 다루는 표준.
   * 자율 주행 시스템은 잠재적 위험이 높으므로, 센서·제어기의 오류 검출, 페일세이프, 인적 개입 가능성 등 안전 요구 사항을 반영해야 한다.
3. **SAE J3016 (자동화된 주행 시스템 분류)**
   * 레벨 0부터 레벨 5까지 자율 주행 기능의 범위를 정의.
   * 기초 수준 자율 주행은 보통 레벨 2 이하(부분 자동화)에 해당하며, GNSS 기반 서행 주행이나 제한 구역 내 자율 주행 정도로 해석할 수 있다.
4. **ROS (Robot Operating System) / Autoware / Apollo 등**
   * 오픈소스 자율 주행 플랫폼으로, 센서 드라이버( GNSS, LiDAR, 카메라 등), 맵핑, 경로 계획, 제어 기능을 통합 제공.
   * 학습 곡선이 있지만, 다양한 패키지를 활용하여 프로토타입 개발 시간을 단축할 수 있다.
5. **TCP/UDP, CAN, RS-232, RS-485, I²C, SPI**
   * 차량 및 센서 간 통신 인터페이스. GNSS 모듈에서 UART(NMEA 0183)나 CAN 출력이 흔히 사용된다.
   * 보조 센서(IMU, 엔코더 등)와 제어기(ECU) 간 데이터 교환 시 프로토콜 선택에 따라 배선, 속도, 안정성이 달라진다.

\--- 정리

* 위 내용들을 종합하면 기초 수준 자율 주행을 위해 GNSS를 기반으로 차량 위치를 추정하고, IMU, 휠 엔코더 등의 보조 센서를 융합하여 더욱 안정된 측위 결과를 얻을 수 있다.
* 추가적으로 간단한 로컬 맵 또는 Waypoint 기반 주행, 장애물 감지를 위한 초음파나 레이더 센서, 기본적인 경로 추종 알고리즘(Stanley, Pure Pursuit, PID 등)을 적용할 수 있다.
* 안전 측면에서 운전자 개입, 페일세이프, 긴급 정지(E-Stop) 장치 등이 필수적으로 고려되어야 하며, 표준·규격을 참조해 시스템 신뢰도를 높이도록 설계한다.
* 시뮬레이션과 실제 필드 테스트를 병행하면서 로그 데이터를 수집·분석해 알고리즘과 파라미터를 개선하는 과정이 중요하다.