(AD) Vehicle Control - Pure Pursuit, Stanley

차량의 기하학 정보를 바탕으로 추종할 경로 사이의 기하학적 관계를 이용해 제어 법칙을 도출하는 Geometric Control 의 대표적인 방법으로 Pure Pursuit 과 Stanley 가 있다.

Pure Pursuit Control

개요

• Pure Pursuit는 차량과 목표 경로 사이의 기하학적 관계를 이용하여 조향각(Steering Angle)을 계산하는 대표적인 횡방향 경로 추종(Path Tracking) 알고리즘이다.
• 차량 동역학 모델이나 복잡한 최적화 과정 없이, 차량과 목표점 사이의 기하학적 관계만을 이용하여 조향각을 계산하므로 계산량이 매우 작고 구현이 간단하여 자율주행 자동차와 모바일 로봇에서 널리 사용된다.

Pure Pursuit 방법

• 차량은 목표점(Target Point)을 향하는 하나의 원호(Circular Arc)를 따라 움직이며, 이 원호의 곡률(Curvature)에 대응하는 조향각을 계산한다.
• 일반적으로 Bicycle Kinematic Model을 기반으로 차량의 후륜축을 기준점으로 사용하여 나타냄.

Lookahead Distance

• 이때, 차량에서 목표점까지의 거리를 Lookahead Distance 라고 하며 이 거리가 짧으면 빠른 조향 응답과 급격한 골률 추종이 가능한 대신 노이즈에 민감하고 급격한 조향으로 인한 진동이 발생한다.
• 반대로, Lookahead Distance가 길면 부드러운 조향으로 안정적인 주행이 가능하지만 급커브(직각 등)에서 코너 안쪽을 가로지르는 현상(coner cutting)이 발생하고 추종 오차가 증가한다.
• 실제 self-drive 시스템에서는 속도가 증가할수록 Distance를 증가시키는 경우가 많다.

회전 경로(원호) 계산

• $d$ : Lookahead Distance
• $\alpha$ : 차량 기준좌표에서 차량 방향과 목표점 방향 사이의 각도
• $R$ : 원호의 반지름
• $\delta$ : 차량 조향각

• 차량은 직선으로 움직일 수 없고, 일정한 조향각에서는 항상 원호(Circular Arc) 를 따라 움직이므로 차량과 목표점을 지나는 원을 생성하여 이로부터 조향각을 구한다.
• 차량의 기준점(후륜축)과 목표점 사이의 관계를 이용해서 원호의 반지름 $R$ 과 곡률 $\frac{1}{R}$을 정의할 수 있다.

$$R=\frac{d}{2\sin \alpha }$$

• 곡률(Curvature)은 회전량이라 할 수 있으며, 반지름이 커질수록 회전량은 작아진다.

$$\kappa =\frac{1}{R}=\frac{2\sin \alpha }{d}$$

조향각 계산

• Bicycle Kinematic Model 에서 조향각 $\delta$는 다음과 같이 표현한다.

$$\tan \delta =\frac{L}{R}$$

• 원호의 반지름 식을 대입하면 아래와 같다.

$$\tan \delta =\frac{2L\sin \alpha }{d}$$ $$\therefore \delta ={\tan }^{-1}\left(\frac{2L\sin \alpha }{d}\right)$$

---

Stanley Control

개요

• Stanley Controller는 2005년 DARPA Grand Challenge 우승 차량인 Stanford의 Stanley에서 사용된 경로 추종(Path Tracking) 알고리즘으로, Pure Pursuit 과 마찬가지로 차량의 현재 상태와 목표 경로 사이의 기하학적 관계를 이용하여 조향각을 계산하는 횡방향 제어기이다.
• Pure Pursuit 은 Lookahead Point만 바라보는 반면, Stanley는 Cross-Track Error(차량이 경로에서 얼마나 벗어나 있는가)와 Heading Error(차량의 진행 방향이 경로 방향과 얼마나 차이나는가)를 최소화하는 방향으로 조향각을 업데이트한다.

Stanley 방법

• Pure Pursuit 은 차량의 후륜축을 기준으로 삼지만, Stanley는 차량의 전륜축을 기준으로 한다.
• $\delta$ : 차량 조향각
• $v$ : 차량 속도
• $e$ : Cross-Track Error
• $\psi$ : Heading Error

Stanley 조향 제어

• Stanley 조향 제어 법칙은 아래와 같이 Heading Error 항과 Crooos-Track Error 항으로 구성되어 있다.

$$\delta =\psi +{\tan }^{-1}\left(\frac{ke}{v}\right)$$

• Stanley 에서 목표점(Target Point)은 전륜축 중심(Front Axle Center)에서 가장 가까운 경로상의 점(Nearest Point) 을 사용한다.

Heading Error

• Heading Error 는 차량의 현재 진행 방향과 경로의 접선 방향 사이의 각도 차이를 의미한다.

$$\psi ={\psi }_{path}-{\psi }_{vehicle}$$

• 차량의 yaw 자세를 경로 방향으로 정렬하기 위해 사용된다.

Cross-Track Error (CTE)

• Cross-Track Error 는 차량의 기준점과 목표점 사이의 오차인 $e$ 와 차량 속도벡터 $v$를 이용해 차량이 경로에서 멀리 떨어져 있으면 빠르게 복귀시키고, 가까우면 부드럽게 복귀시키도록 한다.

$${\theta }_{CTE}={\tan }^{-1}\left(\frac{ke}{v}\right)$$

• $e$가 커질수록 ${\theta }_{CTE}$가 증가하여 조향각이 커지고, $v$가 커질수록(속도가 빠를수록) ${\theta }_{CTE}$가 감소하여 조향각이 작아진다.
• 여기서 $k$는 $e$에 대한 비례 게인(Gain) 상수로, 클수록 차량이 빨리 경로로 수렴하지만 조향 진동이 발생할 수 있다.
• Cross-Track Error의 변화율 $\dot{e}$ 는 차량 속도의 횡성분에 의해 감소한다.

---

Simulation Test

Pure Pursuit + PID Control

• Pure Pursuit을 이용한 조향각 제어와 PID Control을 통한 속도 제어 시뮬레이션 개발.
  • Lateral Control – Pure Pursuit, Longitudinal Control – PID
• Lookahead Distance를 따로 두지 않고, 전방 타겟을 Lookahead Point로 고정함.

---

참고

• (AD) Vehicle Control - Overview
• (AD) Vehicle Control - PID Control
• lec_06_vehicle_control.pdf
• 2-1. Pure Pursuit — 전방 목표점 추종 - 로봇 시뮬레이션
• Pure Pursuit 제어기 - MATLAB & Simulink
• Dynamics & Control – Welcome to Hang’s Website!
• Title Unavailable | Site Unreachable
• isl.ecst.csuchico.edu/DOCS/darpa2005/DARPA 2005 Stanley.pdf