조향 알고리즘

지금까지는 매 프레임 차의 위치를 도로 위로 강제 설정

동작은 하지만

OwnerPawn->SetActorLocation(NewLocation);  // ← 이게 문제

• 차에 충돌 같은 게 와도 무시하고 그냥 도로로 점프
• 가속/감속 같은 물리적 동작이 없음
• 다른 차랑 부딪히는 시나리오 만들 수 없음

→ 차의 위치는 차가 알아서 움직이게 두고 핸들/페달만 조작해야 한다

 

언리얼의 차량 시스템은 보통 이런 함수를 받는다

Vehicle->SetSteeringInput(float Value);  // -1 ~ +1
Vehicle->SetThrottleInput(float Value);  // 0 ~ 1
Vehicle->SetBrakeInput(float Value);     // 0 ~ 1

 

값의 의미:

• 조향: -1 = 완전 왼쪽, 0 = 직진, +1 = 완전 오른쪽
• 스로틀: 0 = 안 밟음, 1 = 풀가속
• 브레이크: 0 = 안 밟음, 1 = 풀브레이크

 

지금 단계에서는 차량 폰이 아직 없으니, 일단 로그로만 출력하면서 알고리즘 검증

void ApplySteeringCommand(float Steering)
{
    // 나중에 실제 차량에 연결
    UE_LOG(LogTeam24, Log, TEXT("Steer: %.3f"), Steering);
}

 

 

그래서 핸들 값을 어떻게 정하지

어떤 기준으로 -1 ~ +1 사이의 값을 정해야 차가 도로를 따라갈까

• 차가 도로의 왼쪽에 있으면 → 오른쪽으로 핸들
• 차가 도로의 오른쪽에 있으면 → 왼쪽으로 핸들
• 차가 도로 정중앙에 있으면 → 직진

도로가 곡선일 때는?

• 도로가 오른쪽으로 꺾인다 → 미리 오른쪽으로 핸들 꺾어야 함
• 그냥 지금 위치만 보면 너무 늦음

 

검색하며 처음 본 용어들:

• Pure Pursuit: 차 앞쪽 한 점을 향해 핸들 돌리기
• Stanley: 도로 중심선과의 거리/각도 기반
• MPC (Model Predictive Control): 너무 복잡해 보임...

 

Pure Pursuit가 가장 만만해 보임

차 앞쪽에 한 점을 정해놓고 (look-ahead point),
그 점을 향해 핸들을 돌린다.

[차]------→[가야할점]
     ↑
   look-ahead distance

 

차 앞에 한 점을 정하고 그쪽을 보고 운전

 

전방 주시 지점 정하기

먼저 차 앞쪽 한 점을 도로 위에서 어떻게 정할지

const float LookAheadDistance = 1500.f;  // 15m 앞
FVector LookAheadPoint = GetLocationAtDistance(CurrentDistance + LookAheadDistance);

 

CurrentDistance + 1500만큼 도로를 따라간 위치

 

차에서 그 점까지의 방향 계산

FVector ToLookAhead = (LookAheadPoint - VehicleLocation).GetSafeNormal();

 

GetSafeNormal()은 벡터를 단위 벡터로 만들어주는 함수. 길이가 0인 경우도 안전하게 처리한다.

 

차의 방향과 비교

 

차가 지금 보고 있는 방향과 가야 할 방향의 각도 차이를 구해야 한다

// 벡터의 X, Y 성분으로 각도(라디안) 계산
float TargetYaw = FMath::Atan2(ToLookAhead.Y, ToLookAhead.X) * (180.f / PI);

 

 

두 각도의 차이 구하기

float YawDelta = TargetYaw - VehicleYaw;

 

문제: 각도는 -180 ~ 180 범위인데 350° - 10° = 340°처럼 큰 값이 나올 수 있다

사실 이건 -20°로 표현해야 맞다

float YawDelta = FMath::FindDeltaAngleDegrees(VehicleYaw, TargetYaw);
// 자동으로 -180 ~ +180 범위로 보정해줌

 

 

핸들 값으로 변환

 

각도 차이를 -1 ~ +1 핸들 값으로 변환해야 함

const float MaxYawDelta = 40.f;  // 40도면 풀로 핸들 꺾는다고 가정
float Steering = FMath::Clamp(YawDelta / MaxYawDelta, -1.f, 1.f);

 

Clamp는 값을 범위 안으로 강제하는 함수

40도 이상은 다 +1로 처리

 

 

문제 발견:

차가 도로 위에서 좌우로 흔들흔들 거림

-> AI에게 물어본 결과 Pure Pursuit는 어딜 향해 갈지만 보지 도로의 진행 방향과 일치하는지는 안 봄

 

 

• 목표 지점만 보고 달릴 때 (Position Error - Pure Pursuit):
  • "저기 앞 도로 중앙점(목표)으로 가야지!" 하고 핸들을 꺾습니다.
  • 문제점: 목표 지점에는 도착하지만, 도착했을 때 차체가 차선과 일직선이 되지 않고 삐딱하게(사선으로) 진입해서 차선을 넘어가며 지그재그로 흔들릴 수 있습니다.
• 도로 방향만 보고 달릴 때 (Heading Error):
  • "무조건 앞 도로가 향하는 방향과 평행하게 달려야지!" 하고 핸들을 맞춥니다.
  • 문제점: 차체가 삐딱하지는 않지만, 만약 차가 갓길에 빠져있다면 영원히 차선 중앙으로 들어오지 못하고 갓길을 평행하게 달리게 됩니다.

 

두 가지 목표를 적절한 비율(가중치, Weight)로 섞어서(Blending) 완벽한 조향 각도를 찾아내야 함!

 

코드 추가

// 전방 주시 지점에서의 도로 방향
FVector LookAheadDirection = GetDirectionAtDistance(CurrentDistance + LookAheadDistance);

// 차의 방향 vs 도로 방향
float HeadingError = FMath::FindDeltaAngleDegrees(
    VehicleYaw,
    FMath::Atan2(LookAheadDirection.Y, LookAheadDirection.X) * (180.f / PI)
);

 

• PositionError (Pure Pursuit) — 가야 할 지점 향하기
• HeadingError — 도로 방향과 일치시키기

const float HeadingWeight = 0.7f;  // 일단 70%로
float YawCmd = PositionError * (1.f - HeadingWeight) + HeadingError * HeadingWeight;
float Steering = FMath::Clamp(YawCmd / MaxYawDelta, -1.f, 1.f);

 

1. const float HeadingWeight = 0.7f; (비율 정하기)

• "도로 방향 맞추기(Heading)에 70%, 목표 지점 찾아가기(Position)에 30%의 신경을 쓰겠다!"라는 뜻입니다. 보통 부드러운 주행을 위해 도로 방향을 맞추는 것에 가중치를 더 높게 줍니다.

2. float YawCmd = PositionError * (1.f - HeadingWeight) + HeadingError * HeadingWeight; (가중 평균)

• PositionError(목표 향하기 오차) * 0.3 + HeadingError(도로 평행 오차) * 0.7
• 즉, 두 개의 조향 각도를 3:7 비율로 섞어 최종적으로 차가 틀어야 할 완벽한 각도(YawCmd)를 만들어냅니다.

3. float Steering = FMath::Clamp(YawCmd / MaxYawDelta, -1.f, 1.f); (핸들 값으로 변환)

• YawCmd는 15도, 30도 같은 '각도'입니다. 하지만 자동차 액터에게 명령을 내릴 때, 핸들(조향, Steering)은 보통 조이스틱처럼 -1.0 (왼쪽 끝)에서 1.0 (오른쪽 끝) 사이의 값만 입력받습니다.
• / MaxYawDelta: 구한 각도를 '자동차가 한 번에 꺾을 수 있는 최대 각도'로 나눠서 비율(0.0 ~ 1.0 근처)로 만듭니다.
• FMath::Clamp(값, 최소, 최대): "값이 아무리 커지거나 작아져도, 무조건 -1.f 와 1.f 사이로 강제로 잘라내라!"라는 뜻입니다. 

 

직선 도로에서 차가 안 흔들림 훨씬 안정적

근데 새로운 문제

• 곡선 도로에서는 차가 도로 안쪽으로 살짝 잘려나감 (커브를 가로지름)
• 도로 정중앙을 따라가지 않고 살짝 안쪽 라인으로 다님

 

횡방향 (Cross-Track) 보정 ->  어려워서 대부분 AI 사용

Pure Pursuit 외에 다른 알고리즘이 있는지 찾아보기

(AI에게 물어봄)

-> stanley 컨트롤러

 

Stanley의 핵심: 도로 중심선에서의 수직 거리 기반 보정

도로 중심선: ─────────────
              ↑
           [차] 5cm 왼쪽으로 벗어남
              ↓
           오른쪽으로 살짝 보정

 

 

횡방향 오차 (Cross-Track Error) 계산

 

-> 차가 도로 중앙선에서 수직으로 얼마나 떨어져 있나?를 풀어야 함

 

핵심 아이디어:

1. 차에서 도로 위 한 점까지의 벡터 (Offset)
2. 도로 진행 방향 벡터 (SegDir)
3. Offset의 SegDir 방향 성분을 빼면 → 도로에 수직인 성분만 남음

 

const FVector RoadPos = GetLocationAtDistance(CurrentDistance);  // 도로 위 가장 가까운 점
const FVector RoadDir = GetDirectionAtDistance(CurrentDistance); // 도로 진행 방향
const FVector Offset = VehicleLocation - RoadPos;                // 차까지 벡터

// Offset의 RoadDir 방향 성분 제거 → 수직 성분
const FVector PerpendicularOffset = Offset - RoadDir * FVector::DotProduct(Offset, RoadDir);

 

 

-> 여기서

const FVector PerpendicularOffset = Offset - RoadDir * FVector::DotProduct(Offset, RoadDir);

 

 

그림을 말로 풀면: 보라색 Offset은 두 부분의 합

• 초록색 = 도로 방향으로 얼마나 갔나 (앞뒤 차이)
• 빨간색 = 도로에서 옆으로 얼마나 벗어났나 (좌우 차이)

원하는 건 빨간색뿐. 그래서:

빨간색 = 보라색 - 초록색
PerpendicularOffset = Offset - (RoadDir 방향 성분)

 

그리고 도로 방향 성분은 내적으로 구함:

도로 방향 성분 = RoadDir × (Offset · RoadDir)

 

여기서 (Offset · RoadDir)은 숫자(스칼라)고, 이걸 단위벡터 RoadDir에 곱하면 그 방향으로의 벡터

 

• Offset (보라색) = 도로 위 점에서 차까지의 화살표
• DotProduct(Offset, RoadDir) = Offset을 도로 방향에 떨어뜨린 그림자 길이 (숫자)
• RoadDir * 그림자길이 = 그 그림자를 화살표로 다시 만든 것 (초록색)
• Offset - 초록색 = 빨간색 (옆으로 벗어난 만큼)

 

 

부호 - 왼쪽 이탈인지 오른쪽 이탈인지

 

빨간 화살표(PerpendicularOffset)는 벡터

길이만 알면 "얼마나 벗어났는지"는 알지만 어느 쪽으로 벗어났는지는 모름

핸들을 어느 쪽으로 꺾을지 결정하려면 부호 있는 숫자가 필요함

• 양수 → 차가 도로 오른쪽으로 벗어남 → 왼쪽으로 핸들 꺾기
• 음수 → 차가 도로 왼쪽으로 벗어남 → 오른쪽으로 핸들 꺾기

아이디어: 도로의 오른쪽 방향 화살표를 하나 만들고 빨간색이랑 내적하자.

 

• 빨간색이 정말 오른쪽이면 → 같은 방향 → 양수
• 빨간색이 왼쪽이면 → 반대 방향 → 음수
  

 

const float CrossTrackError = FVector::DotProduct(PerpendicularOffset, RoadRight);
//          (숫자, 부호 있음) = 내적         (빨간색,        오른쪽 방향)

 

이거 한 줄로:

• 옆으로 얼마나 벗어났는지 (길이)
• 어느 쪽으로 벗어났는지 (부호)

둘 다 한꺼번에 얻음

 

그래서 도로의 오른쪽 방향을 가리키는 벡터가 필요.

이건 외적(Cross Product)으로 구함

 

내적은 결과가 숫자 하나. 외적은 결과가 새 벡터(화살표)

 

손으로 해보기

1. 검지를 앞으로 쭉 펴요 (정면 방향)
2. 중지를 검지에 수직으로 꺾어서 옆으로 가리켜요 (왼쪽 방향)
3. 엄지를 두 손가락에 수직으로 세워요 (위 방향)

이 세 손가락이 서로 다 직각. 이게 외적의 핵심

검지(첫 번째 화살표) × 중지(두 번째 화살표) = 엄지(외적 결과)

세 개 다 서로 수직

 

알고 있는 거:

• 위 방향 (UpVector) — 하늘 쪽
• 도로 진행 방향 (RoadDir) — 차가 가는 쪽

 

세 화살표가 서로 다 직각. 외적이 이걸 자동으로 만들어줌

const FVector RoadRight = FVector::CrossProduct(FVector::UpVector, RoadDir);
//                      = 외적                  (위,               앞)
//             결과: 오른쪽

 

외적은 순서를 바꾸면 방향이 반대가 됨

CrossProduct(Up, RoadDir)  →  오른쪽
CrossProduct(RoadDir, Up)  →  왼쪽 (반대!)

 

언리얼 엔진은 Up × Forward = Right라는 규칙을 씀

 

왜 이걸 굳이 외적으로?

 

"도로가 가는 방향에 수직인 게 옆 방향" 이건 알겠는데, 수직인 방향이 두 개

왼쪽도 도로에 수직, 오른쪽도 도로에 수직.

그래서 그냥 "수직"이 아니라 위에서 봤을 때 오른쪽을 콕 집어서 골라야 함

외적이 위 방향까지 같이 고려하면서 한 방향(오른쪽)을 정확히 찍어주는 것

 

// 1. 도로 위 점, 도로 진행 방향
const FVector RoadPos = GetLocationAtDistance(CurrentDistance);
const FVector RoadDir = GetDirectionAtDistance(CurrentDistance);

// 2. 도로 위 점에서 차까지의 화살표 (보라색)
const FVector Offset = VehicleLocation - RoadPos;

// 3. 보라색에서 도로 방향 성분(초록색) 빼면 옆 성분(빨간색)만 남음
const FVector PerpendicularOffset = Offset - RoadDir * FVector::DotProduct(Offset, RoadDir);

// 4. 외적으로 도로의 오른쪽 방향 만들기
const FVector RoadRight = FVector::CrossProduct(FVector::UpVector, RoadDir);

// 5. 빨간색을 오른쪽 방향에 내적해서 부호 있는 숫자로 변환
const float CrossTrackError = FVector::DotProduct(PerpendicularOffset, RoadRight);

 

 

핸들 보정

float Steering = FMath::Clamp(
    YawCmd / MaxYawDelta - CrossTrackError * CrosstrackGain,
    -1.f, 1.f
);

 

Steering 값은 양수 = 오른쪽 핸들, 음수 = 왼쪽 핸들

차가 오른쪽으로 벗어나 있으면(CrossTrackError > 0), 핸들은 왼쪽으로 꺾어야 함

그래서 빼는 것:

기본 핸들 - (양수) = 더 작아짐 → 왼쪽 방향
기본 핸들 - (음수) = 더 커짐 → 오른쪽 방향

 

부호가 자동으로 맞아 들어감

CrosstrackGain = 0.0015 같은 작은 값을 곱하는 이유는, CrossTrackError가 cm 단위라 숫자가 클 수 있어서

너무 강하게 보정하면 차가 좌우로 휘청거림

 

// 1. 도로 위 가장 가까운 점과 그 점에서의 도로 진행 방향
const FVector RoadPos = GetLocationAtDistance(CurrentDistance);
const FVector RoadDir = GetDirectionAtDistance(CurrentDistance);

// 2. 차에서 도로 위 점까지의 벡터 (양쪽 차이 + 앞뒤 차이가 다 섞여 있음)
const FVector Offset = VehicleLocation - RoadPos;

// 3. 앞뒤 차이를 빼서 좌우 차이만 남김
const FVector PerpendicularOffset = Offset - RoadDir * FVector::DotProduct(Offset, RoadDir);

// 4. 도로의 오른쪽 방향을 외적으로 구함
const FVector RoadRight = FVector::CrossProduct(FVector::UpVector, RoadDir);

// 5. 좌우 차이 벡터를 오른쪽 방향에 내적 → 부호 있는 숫자
const float CrossTrackError = FVector::DotProduct(PerpendicularOffset, RoadRight);

// 6. 핸들에 보정값 추가 (오른쪽 벗어남 → 왼쪽 핸들)
float Steering = FMath::Clamp(
    YawCmd / MaxYawDelta - CrossTrackError * CrosstrackGain,
    -1.f, 1.f
);

 

차에서 도로까지 화살표를 그린다 → 도로 방향 성분을 빼서 옆 성분만 남긴다 → 그걸 도로 오른쪽 방향에 내적해서 부호 있는 숫자로 만든다 → 핸들에서 빼면 차가 도로로 돌아온다.

 

---

새로 발견한 문제

직선 + 곡선 둘 다 잘 되는데, 급커브에서 살짝 어색

지금까지 만든 자율주행 코드는 두 가지 정보를 합쳐서 핸들을 정함

1. PositionError (위치 오차): 지금 옆으로 얼마나 벗어났나? - CrossTrackError
2. HeadingError (방향 오차): 차가 바라보는 방향이 도로 방향이랑 얼마나 어긋났나?

이 둘을 섞어서 핸들을 꺾음

섞을 때 비율을 정하는 게 HeadingWeight

YawCmd = PositionError × (1 - HeadingWeight) + HeadingError × HeadingWeight

 

HeadingWeight = 0.7 (방향 오차 70% 반영, 위치 오차 30% 반영)

 

근데 급커브에서는 차가 도로 방향(= 커브 진행 방향)을 너무 빨리 따라가려고 하면 커브가 휘기 전에 미리 휘는 것!

-> 커브에서는 지금 위치를 더 봐야 함

-> 즉 HeadingWeight를 낮춰야 함

 

곡률을 측정해서, 직선이면 HeadingWeight 높이고, 커브면 낮추자.

 

곡률 측정 - AI 도움

곡률 측정 : 두 지점에서의 도로 방향이 얼마나 달라?

• 직선이면 두 방향이 거의 똑같음 → 각도 차이 ≈ 0
• 살짝 휜 곡선이면 살짝 다름 → 각도 차이 작음
• 급커브면 많이 다름 → 각도 차이 큼

이 각도 차이가 곡률.

 

const FVector D1 = GetDirectionAtDistance(CurrentDistance + AheadOffset);
const FVector D2 = GetDirectionAtDistance(CurrentDistance + AheadOffset + 50.f);

 

전방의 어느 지점에서 도로 방향을 두 개 가져옴.

두 지점은 50m 떨어져 있음.

return FMath::Acos(FMath::Clamp(FVector::DotProduct(D1, D2), -1.f, 1.f));

 

이게 핵심. 풀어서 보면:

 

1단계: 내적

FVector::DotProduct(D1, D2)

 

D1과 D2는 단위벡터 (길이 1).

단위벡터끼리 내적하면 cos(각도) 가 나옴

 

내적의 일반 공식:

A · B = |A| × |B| × cos(θ)

 

|A| = |B| = 1이면:

A · B = cos(θ)

 

즉:

• 두 방향이 같으면 (각도 0°) → cos(0°) = 1
• 직각이면 (각도 90°) → cos(90°) = 0
• 반대 방향이면 (각도 180°) → cos(180°) = -1

 

2단계: Acos로 각도 복원

FMath::Acos(...)

 

cos 값에서 각도를 거꾸로 복원하는 함수

• cos = 1 → Acos → 0° (직선)
• cos = 0.99 → Acos → 작은 각도 (약한 곡선)
• cos = 0.5 → Acos → 60° (급커브)

결국 각도(라디안) 가 나옴.

이게 곡률 근사값!

 

3단계: Clamp로 안전장치

FMath::Clamp(..., -1.f, 1.f)

 

부동소수점 계산은 가끔 오차가 생김

두 단위벡터가 거의 같으면 내적이 정확히 1이어야 하는데, 컴퓨터가 1.0000001 같은 값을 뱉을 때가 있음

Acos(1.0000001)은 수학적으로 정의 안 됨 → NaN(Not a Number) 반환 → 코드가 망가짐

Clamp(x, -1, 1)은 x를 -1과 1 사이로 강제로 제한

1.0000001이 들어오면 1로 잘라줌

 

가중치 동적 조정

1단계: 곡률 측정

const float CurvHere = EstimateCurvature(0.f);

 

지금 이 지점의 곡률(라디안)을 구함

 

2단계: 0~1 사이로 정규화

const float CurvNorm = FMath::Clamp(CurvHere * 3.f, 0.f, 1.f);

 

곡률 값은 라디안인데, 보통 0.0 ~ 0.3 정도 범위

이걸 다음 단계에서 쓰기 편하게 0~1 사이로 만들기

• × 3.f: 0.33 라디안만 돼도 1로 만들기 위해
• Clamp(0, 1): 1을 넘어가면 1로 자르기

결과:

• 직선이면 CurvNorm = 0
• 적당한 곡선이면 CurvNorm = 0.5
• 급커브면 CurvNorm = 1

3단계: Lerp로 가중치 보간

const float HdgW = FMath::Lerp(0.7f, 0.3f, CurvNorm);

 

Lerp(A, B, t) 는 A와 B 사이에서 t만큼 위치한 값을 줌

• t = 0이면 A
• t = 1이면 B
• t = 0.5면 중간 (A와 B의 평균)

수식으로:

Lerp(A, B, t) = A + (B - A) × t

• CurvNorm = 0 (직선) → HdgW = 0.7 (방향 많이 봄)
• CurvNorm = 1 (급커브) → HdgW = 0.3 (위치 많이 봄)
• CurvNorm = 0.5 → HdgW = 0.5 (반반)

 

전방 주시 거리도 같은 원리

const float CurvScale = FMath::Lerp(1.f, 0.5f, CurvNorm);
const float LADist = (LookAheadBase + Speed * 0.3f) * CurvScale;

 

전방 주시 거리: 차가 도로 위 어느 점을 보고 따라갈지 정하는 거리.

• 직선에서는 멀리 보면 안정적 (휘청거림 줄음)
• 커브에서는 가까이 봐야 정확함 (멀리 보면 안쪽 자르기)

같은 Lerp 패턴:

• 직선 (CurvNorm = 0) → CurvScale = 1.0 → 100% 거리
• 급커브 (CurvNorm = 1) → CurvScale = 0.5 → 50% 거리

LookAheadBase가 기본 거리고, Speed * 0.3f는 속도가 빠를수록 더 멀리 보는 효과

거기에 CurvScale을 곱해서 커브에서 단축

 

곡률 = 두 지점 도로 방향의 각도 차이 → Lerp로 가중치/거리 조절 → 직선에선 멀리 보고 방향 따라가고, 커브에선 가까이 보고 위치 정확히 잡기.

 

---

<최종 정리>

// 1. 3가지 오차 측정
PositionError    = 차 방향 vs 가야할 점 방향
HeadingError     = 차 방향 vs 도로 방향
CrossTrackError  = 도로 중심선 이탈 거리

// 2. 곡률에 따라 가중치 결정
HdgWeight = 직선이면 0.7, 급커브면 0.3

// 3. 블렌딩
YawCmd = PositionError * (1-HdgW) + HeadingError * HdgW
Steering = YawCmd / MaxYawDelta - CrossTrack * Gain

 

USplineComponent

GetLocationAtDistanceAlongSpline	거리 → 위치
GetDirectionAtDistanceAlongSpline	거리 → 진행 방향

FMath

FMath::Atan2(Y, X)	벡터 → 각도 (라디안)
FMath::FindDeltaAngleDegrees	두 각도의 차이 (-180~180 보정)
FMath::Clamp	값을 범위 안으로
FMath::Lerp(A, B, t)	선형 보간
FMath::Acos	코사인의 역함수

FVector

GetSafeNormal()	단위 벡터로 (0 안전)
FVector::DotProduct	내적 (투영 길이)
FVector::CrossProduct	외적 (수직 벡터)

 

<참고 문서>

https://en.wikipedia.org/wiki/Pursuit_curve

Stanley method 정리

Atan2 | Unreal Engine 5.3 Documentation | Epic Developer Community

FMath | Unreal Engine 5.7 Documentation | Epic Developer Community

 

 

이번에 알게 된 자율주행 알고리즘

Pure Pursuit

• 차 앞쪽 한 점을 정해놓고 그쪽을 보고 핸들 조작
• 단순하고 직관적
• 단점: 진동, 안정성 부족

Stanley Controller

• 도로 중심선과의 수직 거리 + 각도 차이로 핸들 조작
• 안정적이지만 직관성 떨어짐

최종 선택 : 하이브리드

• 둘의 장점만 합침
• Pure Pursuit (위치) + Heading (방향) + Stanley (횡방향) = 안정적이고 정확함