[AD] Cubic Spline Interpolation (3차 곡선 보간법)

Author : Juntae Park

Date : 2022/09/15

Source

- 수학적 설명 Blog : https://seong6496.tistory.com/193

[수치해석] Spline interpolation(스플라인 보간법)

- Python Implementation : https://github.com/AtsushiSakai/PythonRobotics/tree/master/PathPlanning/CubicSpline

GitHub - AtsushiSakai/PythonRobotics: Python sample codes for robotics algorithms.

- Related Paper : Optimal trajectory generation for dynamic street scenarios in a Frenét Frame (M. Werling, 2010 ICRA)

 

---

0. Motivation

 자율주행 Planning에 대해서 공부하는 중, Frenet Frame에서 경로 계획을 수행하는 것을 확인해보았습니다. 이를 Python 코드로 구현한 PythonRobotics Github 소스코드 분석 중, Frenet Frame의 기준이 되는 Reference Waypoint 들을 Cubic Spline으로 보간하는 코드를 분석하면서 수학적으로 어떻게 보간하는지 직접 수식을 확인해보려고 합니다.

 

1. Algorithm

구체적인 수학적 증명 :

Cubic Spline Planner.pdf

1.21MB

0. Input : 기준이 되는 Reference Waypoints $[[x_0, y_0], [x_1, y_1], \cdots [x_{n-1}, y_{n-1}]]$

 

1. 근사 곡선의 길이 $s$ 계산

$ s_0 = 0$

$ s_i = \sqrt{(x_i - x_{i-1})^2 + (y_i - y_{i-1})^2}$

$ s = [s_0, s_1, \cdots s_{n-1}] $

 

2. $x$, $y$ 각각을 곡선의 길이에 대한 함수 $x(s)$ $y(s)$ 1D Cubic Spline으로 분리하여서 Interpolation

 

3. 1D Cubic Spline으로 Interpolation

• $ f_k(s) = a_k + b_ks + c_ks^2 + d_ks^3 $
• 만약 점이 n개 있다고 하면, n-1개의 함수 $f_k(s)$ $f_k(s)$ $ k = 0 \sim n-2$ 개의 함수로 구성
• 계수 찾기 : 미지수 4n-4개
  • 인접 함수 $f_{k-1}(s)$   $f_{k}(s)$ 끼리 2계 미분 연속 → 3n-6개의 방정식
  • 초기 위치 $f_{k}(0) = x_{k} $ → n-1개의 방정식
  • 마지막 방정식의 종료 위치 $f_{n-1}(s_{n-1}) = x_{n-1}$ → 1개
  • Natural Cubic Spline 조건 → 2개
    • $\frac{d^2}{ds^2}x_0(s_0) = 0$
    • $\frac{d^2}{ds^2}x_{n-1}(s_{n-1}) = 0$
• $c$에 대한 식로 정리 후 $Hc = g$ 형태로 변분하여 모든 계수 계산

\[\begin{bmatrix}
1 & 0 & 0 & \cdots & 0 & 0 \\
h_0 & 2(h_0 + h_1) & h_1 & \cdots & 0 & 0 \\
0 & h_1 & 2(h_1 + h_2) & h_2 & \cdots & 0\\
\vdots & && \ddots  & & \vdots \\
0 & 0 & \cdots & h_{n-3} & 2(h_{n-3}+h_{n-2}) & h_{n-2}\\
0 & 0 & \cdots & 0 & h_{n-2} & 2(h_{n-2}+h_{n-1})
\end{bmatrix}
\begin{bmatrix} c_0 \\ c_1\\ c_2 \\ \vdots \\ c_{n-3} \\ c_{n-2} \end{bmatrix}
=
3 \begin{bmatrix} g_1-g_0 \\ g_2-g_1 \\ g_3 - g_2 \\ \vdots \\ g_{n-3}-g_{n-4} \\ g_{n-2}-g_{n-3} \end{bmatrix}
\]

 

$ g_i = \frac{a_{i+1} - a{i}}{h_{i}} ( i = 0 \sim n-2) $

$ h_i = s_{i+1} - s_{i} (i = 0 \sim n-2) $

 

4. $x(s)$ 와 $y(s)$에 대한 1D Cubic Spline 각각을 구한 후 Global 위치, 각도, 곡률 계산

• Position x, y : $x(s), y(s)$
• Yaw $\theta(s)$ : $tan^{-1}(\frac{dy}{dx})$
• Curvature $\kappa(s)$ : $\frac{y''x'-y'x''}{(x'^2 + y'^2)^{3/2}}$

 

2. 마치며...

수식을 직접 써보고 코드 Error 분석하면서 간단해 보이는 Idea도 코드로 버그없이 돌아가게 하는게 쉽지 않은걸 느껴졌네요. Cubic Spline을 이용해서 Frenet State에서 경로 계획을 수행하는 코드 분석과 수식 확인을 다음으로 해볼 수 있었으면 좋겠습니다.