Pure Pursuit <Path Tracking Algorithm>

• 개요

Pure Pursuit 알고리즘은 Path Tracking & Lateral Control 알고리즘이다.

Stanley Method와 함께 시뮬레이션이나 자율주행 모빌리티의 횡제어에 사용되는 간단한 알고리즘이다.

알고리즘은 간단하기 때문에, 실제로 좋은 성능을 내기 위해서는 튜닝 파라미터를 잘 설정하거나, 수식적으로 보완이 필요하다.

 

• 순서도

 

• 설명

\begin{align} &L \, :\,  Vehice\, Length\, or\, Wheel\, Base \\& \delta \, : \, Wheel \, Steering \\& R \, : \,  Radius \, of \, circle \\& d \, : \, distance \, between \, vehicle \, position \, (rear \, wheel) \, and \, look \, ahead \, point \\& Look \, ahead \, point \, coordinate \, : \, vehicle \, local \, coordinate, \, origin \, point \, is \, in \, rear \, wheel \end{align}

 

 

pure pursuit 알고리즘의 출력은 차량의 뒷 바퀴가 Look Ahead Point에 도달하기 위한 wheel Steering

수식 계산은 다음과 같다.

 

\begin{align}&tan(\delta) \, = \, \frac{L}{R} \, \to \, \delta \, = \, arctan(\frac{L}{R}) \tag{1} \\& d^2 \, = \, x^2 \, + \, y^2 \tag{2-1} \\& x \, + \, k \, = \, R \tag{2-2} \\&\to \, k \, = \, R \, - \, x \\&\to (R \, - \, x)^2 \, +\,  y^2 \, = \, R^2 \\&\to -2Rx \, +\,  x^2 \, + \, y^2 \, = \, 0 \\&\to d^2 \, = \, 2Rx \,\,\, \leftarrow  (2-1) \\&\to R \, = \, \frac{d^2}{2x} \\&\delta \, = \, arctan\, (\frac{2x}{d^2} \, \cdot \, L) \leftarrow \, (1)  \end{align}

 

• 특징

Pure Pursuit 알고리즘은 차량을 Bicycle Model 혹은 Ackerman Model로 가정한다.

그리고 차량의 제어 추종 기준점을 Rear Center Wheel로 가정한다.

해당 알고리즘은 제어 추종 기준점을 Look AHead Point로 일치시키게끔 제어하기에,

위치 값 (x,y)만 제어되고 차량의 Heading 과 경로의 기울기는 일치되게끔 제어되지 않는다.

이는 곡률이 큰 경로에서 단점이 될것으로 보이고, Stanley Method가 이 단점을 일부 해소할 것으로 생각된다.

Look Ahead Point 까지의 거리가 매우 작아 0과 가까울 경우, Steering 계산시 발산할 수 있다. (Look Ahead Point 까지의 거리 값이 분모에 있기 때문) 이를 방지하기 위해 해당 거리 값을 늘 특정 값 이상으로 설정하거나, 분모에 Parameter 값을 더할 수 있다.