Optimal Frenet planning algorithm

정승균·2021년 2월 4일

자율주행 제어

목록 보기

7/8

Frenet Frame상에서 polynomial path를 생성
횡방향 : initial value $x_i, v_i, a_i$ 와 target value $x_f, v_f, a_f$ 와 target time $T$ 로 5차 다항식 계산
종방향 : initial value $x_i, v_i, a_i$ 와 target value $v_f, a_f$ 와 target time $T$ 로 4차 다항식 계산

\dot s (t) \le \dot s_{limit}

\ddot s (t)^2 + \ddot d (t)^2 \le a^2_{max}

\kappa \in [\kappa_{min}, \kappa_{max}]

종방향 Cost
- longitudinal jerk : 종 방향 안정감
- veltocity deviation : 타켓 속도와 얼마나 근접하였는지
- time interval : 얼마나 빨리 조건에 도달하였는지 $J_s = c_{j,s} \int_{t_i}^{t_f} (\dot{a_s}(t))^2dt \; + c_{v,s}(\dot s_{set} - \dot s_f)^2 + c_{T,s}T$
횡방향 Cost
- lateral jerk : 횡 방향 안정감
- time interval : 얼마나 빨리 조건에 도달하였는지
- terminal lateral deviation : 이전 optimal 경로와의 변화, 경로 떨림 방짐 $J_d = c_{j,d} \int_{t_i}^{t_f} (\dot{a_d}(t))^2dt \; + c_{T,d}T + (d_f - d_{f, opt})^2$
Total Cost
$J_{tot} = w_sJ_s + w_dJ_d$