문제 정의

• input (주어진 정보)
  • delta t초 간격으로 M개 point의 궤적이 있음.
  • 다만, point 당 정보는 x,y,yaw만 있는 상태
• output (구하고 싶은 것)
  • 위 궤적에서, 각 point당 velocity, acceleration 등을 구하고 싶음

---

---

---

방법 1: LQR 코드에서 reference point 잡는 방식

• 함수: get_velocity_curvature_profiles_with_derivatives_from_poses

1. velocity, acceleration 구하기

• 여기서 속도, 가속도는 각각 스칼라 값이다. x , y 방향을 따로 구하지 않는다.

---

1.1. _fit_initial_velocity_and_acceleration_profile

• 목적
  • 첫 점의 속도와 모든 점의 가속도를 구하는 함수
  • 이는 각 미래 점의 위치 변화량을 잘 성명하기 위함이다.
• 가정
  • 각 구간 당 (delta t초 길이) 등가속도 운동을 가정한다.
• 방식
  • 

---

1.2. _generate_profile_from_initial_condition_and_derivatives

• 목적
  • 위 함수에서 구한 output으로부터, 모든 지점의 속도를 구한다.
• 방식
  • 등가속도 운동공식을 통해 쉽게 도출

---

2. curvature와 curvature rate 구하기

2.1. _fit_initial_curvature_and_curvature_rate_profile

• 목적
  • 첫 점의 curvature와 모든 점의 curvature rate를 구하는 함수
  • 이는 각 미래 점의 heading 변화량을 잘 설명하기 위함이다.
• 방식
  • $\Delta heading \approx v * curvature * \Delta t$ 수식 활용
• 
• 각 구간(delta t)에서, curvature rate가 일정하다는 가정
• 

---

2.2. _generate_profile_from_initial_condition_and_derivatives

• 목적
  • 위에서 구한 정보들을 이용해서, 모든 점의 curvature을 구한다.

---

• 자 이제, 모든 future trajectory의 속도와, 곡률을 구했다.

3. 추가 정보: reference point 구하기

• _compute_reference_velocity_and_curvature_profile
• 목적
  • 위에서 구한 velocity_profile, curvature_profile을 바탕으로,
  • 현재 시점 + lookahead 만큼 떨어진 시점에서의
    • 아래 2개를 구한다.
      • 속도 : reference_velocity를 보간(np.interp)으로 찾는다.
      • 곡률 : reference_curvature_profile을 추출한다. (np.interp 사용)
    • 참고: lookahead 점 선정 방법
      • (= current_iteration.time_point.time_s + tracking_horizon(=10 point) * discretization_time)

3.1. 구체적 방법

• 속도와 곡률 모두 선형 보간 하는게 전부임

---