Lateral Control

Stanley 알고리즘

• Stanley Algorithm : 주행중인 차량이 정해진 Target Path를 추종하기 위한 주행 방향.
  즉 Steering angle $\delta$의 결정 logic.
  ➡️ $\theta_e$ : yaw error. steering angle을 yaw error만큼만 돌리면, 차량은 target path에 도달할 수 없음.
  (두 접선이 평행하기 때문)
  따라서 $\alpha == arctan2(\frac{kx(t)}{v(t)})$만큼 더 꺾어야 한다.➡️ 차량이 Target Path를 많이 벗어날수록 ➡️ $x$ 증가 ➡️ $\alpha$ 증가 ➡️ Steering Angle($\delta$) 증가
  ➡️ 속력(v)이 높을수록 ➡️ $\alpha$ 감소 ➡️ Steering Angle($\delta$) 감소
  
  
  • $arctan$ vs $arctan2$ (https://gammabeta.tistory.com/2500)
    

Stanley Simulation (3.1. ~ 3.6.)

동작 Diagram

1. 초기값 설정

• 차량의 초기 위치 : x position, y position ➡️ (0, 0)
• Steering 제한 조건 : -40.95 ~ 40.95 (deg)
• Target Path 생성 : way point amplitude, wave_length, offset of path ➡️ Sine Wave
  시간 영역을 공간 영역으로 바꿔야 함.
  : $y = sin(2\pi ft)$ ➡️ $sin(2\pi \frac{1}{\lambda}*path_x)$
  

2. 밑그림 그리기

3.1. Closet point 검색

• 차량으로부터 가장 가까운 점 $P_c$를 찾는다.
  

3.2. Lateral Distance X 계산 (크기)

• Target Path의 직선과 차량의 Heading 직선의 이탈 거리 $x$의 크기를 계산.
• Closet Point $P_c$를 지나는 직선의 방정식 $fn_{path}$은 다음과 같이 계산한다.
  $y = slope_{path}(x - cx_1) + cy_1$
  

3.2. Lateral Distance X 계산 (부호)

• 차량이 $fn_{path}$(Target Path)의 왼쪽에 있으면 우회전, 오른쪽에 있으면 좌회전을 해야 하는데...
  차량이 Target Path의 왼쪽에 있는지? 오른쪽에 있는지? 어떻게 알 수 있을까?
  
  ➡️ Target Path에서 $P_c$의 접선의 방정식 $fn_{path}$는 WORLD 좌표계에서 $\theta_{path}$만큼 기울어져 있다.
  ➡️ $\theta_{path}$를 x축과 일치하도록 $P_c$를 기준으로 -$\theta_{path}$만큼 회전변환을 수행한다.

1. case 1 == 회전 변환된 새로운 좌표계에서 차량 앞바퀴 중심점의 $y'$가 (-)에 있는 경우 :
   차량이 Target Path의 오른쪽에 있었으니 좌회전해야 한다.
2. case 2 == 회전 변환된 새로운 좌표계에서 차량 앞바퀴 중심점의 $y'$가 (+)에 있는 경우 :
   차량이 Target Path의 왼쪽에 있었으니 우회전해야 한다.
   

3.3. Lateral Distance Error $\alpha$ 계산

3.4. Yaw Error $\theta_e$ 계산

• Yaw Error $\theta_e = \theta_{path} - \theta_{vehicle}$
  

3.5. Steering angle $\delta$ 계산

• Steering angle $\delta = \theta_e + \alpha$
  

3.6. Constraints check

• 3.1.에서 정의한 차량의 조향 제약 조건(Steering 각속도 limit?, Steering angle limit?)을
  만족하는지 check

3.7. 새로운 차량 위치 계산

3.8. 그림 update

실습 Github

• https://github.com/LeeHyungSeop/3-1_SensorAndActuator/tree/main/12weekth_Stanley