[Paper Review] Trajectory Planning in Frenet Frame via Multi-Objective Optimization

 

https://ieeexplore.ieee.org/stamp/stamp.jsp?arnumber=10180226

https://www.researchgate.net/publication/372335250_Trajectory_Planning_in_Frenet_Frame_via_Multi-Objective_Optimization

---

0. Motivation

Trajectory Planning 기법 중 Frenet Frame을 알게 되어서 어떤 방법을 통해 장점을 얻는지 살펴보겠습니다!

---

1. Instruction

 

1-A. Related Work

1-B. Contributions

위의 장단점을 고려해 trajectory planning 문제를 실시간으로 해결하기 위해

이산 최적화 방법(discrete optimization methods)에 기반한, 정적 장애물 회피를 위한 새로운 다중 목적 경로 계획 알고리즘을 제안하겠다.

 

1. 복잡한 3차원 모션을 표현하기 위해, Frenet frame을 사용한다. (longitudinal movement는 주행 guideline을 따른 세로 방향의 움직임 / lateral movement는 도로 중앙선에 수직인 측면의 움직임)

→ 상태 공간을 이산화한다. (경로에 대하여 시작 위치와 끝 위치를 생성)

→  궤적 해결 공간을 생성한다. (Trjaectory solution space는 5/4차 다항식 (quintic, quartic polynomials)을 사용하여 샘플링된 종료 조건을 초기 조건에 연결해 두 움직임 모두에 대해 생성.)

→  longitudinal과 lateral trajectories는 결합되어 trajectory sets을 생성한다.

→  주행 가능한 trajectories 후보 세트는 차량 운동학적 충돌-회피 제약들을 통해 스크리닝된다.

→  마지막으로, 최적 경로 계획을 위한 사전 정의된 비용함수를 최소화하여 최적의 궤적을 선택한다.

2. 안전한 경로 후보를 평가하기 위해, 장애물 크기를 고려할 수 있는 새로운 특징(novel feature)으로 포함하고, Gauss-Laplace 연산자의 변화를 통해 각 후보 경로의 안전 loss를 계산한다.

3. 제안한 알고리즘의 성능을 4가지 교통 시나리오에서 입증한다. (직선, 곡선, 교차로, U자 도로)

(감소량 : 직선 도로 ((J_{jerk} = 13.47))%, ((J_{offset} = 63.72))% , 곡선 도로  ((J_{jerk} = 32.19))%, ((J_{offset} = 13.86))% , 교차로  ((J_{jerk} = 59.36))%, ((J_{offset} = 44.36))% , U자 도로  ((J_{jerk} = 18.60))%, ((J_{offset} = 45.56))% )

---

2. Frenet And Cartesian Frames

2-A. Frenet Frame for vehicle motion

 

중앙선이 reference line이자 s-axis가 되고,

referenc line의 수직방향을 d-axis라고 한다.

 

이때 s-axis가 lane line과 평행하므로, 차량의 움직임과 도로 간의 상관관계를 표현하는 데 더 쉬워진다.

즉, 기존 Cartesian frame과 비교해보면, Frenet frame은 "도로의 곡률을 고려하지 않아도" 중앙선으로부터 차량의 offset 거리 혹은 lane line을 따른 주행을 계산하는 데 쉬워진다. 

2-B. from Frenet Frame to Cartesian Frame

다른 차량 때문에 경로를 수정해야 할 때,

 

1. Cartesian frame : (( [\overrightarrow{x},\theta _x, \kappa _x, v_x, a_x] ))를 통해 차량의 현재 state를 표현한다.

 

((\overrightarrow{x}))는 Q(x,y)에서의 차량의 위치를 나타내고,

Q에서의 차량의 운동 궤적의 단위 법선, 접선 벡터인 (( \overrightarrow{n_x})), (( \overrightarrow{t_x}))가 있다.

((\theta_x))는 (( \overrightarrow{x}))와 x축과의 각도를 표현하고,

((\kappa_x))는 Q의 곡률을 말하며,

((v_x, a_x))는 각각 속도와 각속도를 표현한다.

 

2. Frenet frame : ((  [s, \dot{s}, \ddot{s}, d, \dot{d}, \ddot{d}, d', d''] ))로 쉽게 표현된다.

s : reference line의 시작점으로부터 P까지의 곡선 거리 (longitudinal displacement)

d : P와 Q 간의 거리로, 횡방향 변위(transverse displacement)

 

여기선 차량의 위치 Q를 reference line로 사영시킨 점 P를 통해 표현한다.

각도 ((\theta_r)) 는 ((\overrightarrow{r}))과 x축 간의 각도를 말하고,

P에서의 reference line의 단위 법선, 접선 벡터인 (( \overrightarrow{n_r})), (( \overrightarrow{t_r}))이 있다.

 

3. from Frenet Frame to Cartesian Frame

(( \left\{\begin{matrix} x_x=x_r - d sin\theta _r
 \\ y_x=y_r+dcos\theta _r
 \\ \theta_x= arctan(\frac{d'}{1-\kappa _r d})+\theta_r \in  [−\pi , \pi]
 \\ v_x=\sqrt{[\dot{s} (1-\kappa_r d)]^2 + (\dot{s}d')^2}
 \\ a_x= \ddot{s} \frac{1-\kappa _r d}{cos\Delta \theta } \cdot  [d'(\kappa _x \frac{1-\kappa _r d}{cos\Delta \theta} - \kappa_r) - \kappa'_r d + \kappa_r d']
 \\ \kappa _x=((d′′ + (k'_rd+k_rd′)tan\Delta\theta)\frac{cos^2\Delta \theta}{1-\kappa_r d}+\kappa_r)
\end{matrix}\right. )) ... (1)

여기서

(( \Delta\theta = \theta_x - \theta_r ))

(( \kappa_r ))는 P점에서의 곡률

---

3. Proposed Algorithm

3-A. Algorithm Overview

Cartesian frame이 아닌 Frenet Frame을 통해 lateral과 longitudinal motion을 분리하여 차량의 움직임 계산을 줄인다.

나아가, 차량 주행 환경은 non-convex problem으로 최적 해를 얻을 수 없기 때문에, 모션 공간을 이산화하여 convex 문제로 변환한다.

마지막으로, Cost Function 및 Constraint에 따라 최적 경로를 계산한다.

* Convex : Convex를 Fuction을 대상으로 말하면 흔히 아래로 볼록한 함수를 말하고, set을 대상으로 말하면 집합 내 두 점을 연결한 선분이 집합에 포함되면 convex.

* non-convex problem : 경사하강법을 통해 전역 최적해를 구하곤 하는데, non-convex라면 학습이 잘 안되고, 지역 최저점에 갇히는 문제 등이 발생.

 

 

제안된 프레임워크는 2단계로 구성되어 있다.

1. Frenet Frame에서 Trajectory 생성

2. 최적 Trajectory 선택

 

먼저 Frenet Frame에서 Trajectory 생성에서 주요 단계를 알아보자.

1) Spaital Discretization (공간 이산화)

도로 폭은 d축을 따라, ((\Delta d)) 간격으로 파라미터화되고,

도로 upper 경계는 ((d_{roadwidth})) 도로 폭이며,

lower 경계는  ((-d_{roadwidth})) 도로 폭이다.

2) Start/End status Initialization (시작/종료 상태 초기화)

시작, 종료 궤적점의 상태는 경로 생성에 영향을 미친다.

프레넷 프레임은 차량의 움직임을 횡방향 및 종방향으로 분리하기 때문에,

d축을 따라 시작 및 종료 상태는  ((  [d_s, \dot{d_s}, \ddot{d_s}, d_i, \dot{d_i}, \ddot{d_i}] )) 로 표시되며,

여기서 ((d_s))는 d축을 따른 시작 궤적점의 좌표이고, ((d_i))는 d축을 따른 종료 궤적점의 좌표이다.

 

본 논문에서는 차량의 속도가 정방향으로 유지되므로

s축을 따른 시작 및 종료 상태는 (( [s_s, \dot{s_s},\ddot{s_s}, \dot{s_i}, \ddot{s_i}] )) 이고,

여기서 ((s_s))는 s축을 따른 시작 궤적점의 좌표이고, ((s_i))는 d축을 따른 종료 궤적점의 좌표이다.

 

왜 d축과 s축 각각 상태 크기가 다를까? : s축의 방향의 경우 속도가 정방향으로 유지된다고 가정하므로, 종점의 위치 구성을 무시한다.

3) Trajectory Generation

1. d축 및 s축 궤적 방정식은 각각 5차 다항식과 4차 다항식을 사용하여 해결됨

2. planning cycle T를 ∆ t로 이산화 → 얻은 시계열 [0, ∆ t, ..., T]를 d축 및 s축 궤적 방정식에 넣고 →  각 궤적에서 궤적 포인트의 d축 및 s축 좌표를 계산

3. 해당 d축 및 s축 좌표를 결합하여 계획된 궤적을 얻습니다.

 

자세한 내용은 섹션 III-B 및 섹션 III-C(Lateral / Longitudinal Motion Trajectory Planning)에서 시연된다.

 

 

 

앞서 Frenet Frame에서 궤적 생성을 봤다면,

이제 최적의 궤적 선택 단계에서 주요 단계 영역을 살펴보자.

4) Trajectory Point Coordinate Transformation

 

앞서 본 방정식 (1)을 통해, Frenet Frame임에서 생성된 궤적은 전역 Cartesian frame에 다시 매핑된다.

5) Trajectory Check

궤적은 기본적으로 공간-시간 정보(spatial-temporal information)로 구성되어 있다.

(i.e. 위치(location), 곡률(curvature), 헤딩(heading), 속도(velocity) 및 가속도(acceleration) 등)

 

본 논문에서는 trajectory check에서 경로 상의 각 궤적 지점의 충돌, 가속도 및 곡률만 검사한다.

 

앞서 III-A2 섹션과 III-A3 섹션을 통해 궤적 방정식과 궤적 점을 구했다.

이 단계에서는 각 궤적 점에 가상 ego car를 매핑하고, 아래 그림 4(b)와 같이 가상 ego 경계 간 충돌 여부를 확인한다.

차량 운동학(kinematics)과 동역학(dynamics)의 한계를 고려하여. 궤적 상의 각 궤적 지점의 가속도와 곡률을 hard constraints을 사용하여 확인한다.

 

<충돌이 없고, 가속 및 조향 제약 조건을 만족>할 수 없는 궤적 지점이 있는 경우 궤적은 걸러진다.

자세한 작동 및 도출 프로세스는 III-D 섹션(Candidate Path Generation)에서 논의된다.

이 단계를 통해 후보 궤적을 생성하고 충돌 없는 궤적을 생성한다

 

6) Optimal Trajectory Selection

각 궤적의 총 손실이 계산되고, 총 손실이 최소인 궤적이 최적 궤적으로 선택된다.

비용 함수는 세가지 지표로 구성되는데, 이에 대해서는 III-E절에 자세히 정의되어 있다.

(comfort, trajectory safety, and trajectory anti-deviation)

편안함, 궤도 안전성, 궤도 이탈 방지

7) The start point Status Update

planning frequency는 1/∆t로 표시된다.

ego vehicle이 전진함에 따라 위치가 계속 변하므로, 따라서 trajectory planning 접근 방식에서는 차량의 현재 위치를 다음 궤적을 생성하기 위한 기준점으로 사용한다.

차량의 현재 위치를 (( (x_0, y_{m0}) ))으로 지정하고 현재 위치보다 ∆t 초 앞선 궤적 지점(( (x_1, y_{m1}) ))을 계산한다.

이 지점은 다음 궤적 계획 라운드의 새로운 출발점 역할을 한다.

 

 

 

 

3-B. Lateral Motion Trajectory Planning

횡방향 운동 계획(Lateral Motion Trajectory Planning)은 장애물 회피 및 차선 변경(lane cahnge) 작업을 포함합니다.

식 2와 같이, 횡방향 운동 궤적 d(t)는 5차 다항식: (( d(t) = c_{d0}+c_{d1}t+c_{d2}t^2+c_{d3}t^3+c_{d4}t^4+c_{d5}t^5 ))에 기초하여 모델링될 수 있다.

이를 시작점 ((t_s))과 종료점 ((t_e))에 대하여 도입하면 state를 정의할 수 있다.

위의 식 (5)에 대해서 계수를 중심으로 다시 정리하면 아래와 같다.

3-C. Longitudinal Motion Trajectory Planning

본 논문에서는 종방향 운동 궤적을 계획할 때 차량의 속도가 기준선 방향을 따라 일정하게 유지된다고 가정한다.

따라서 일단 종점의 위치 구성은 무시할 수 있으며, 궤적은 수학식 (( s(t) = c_{s0}+c_{s1}t+c_{s2}t^2+c_{s3}t^3+c_{s4}t^4 )) 와 같이 4차 다항식으로 나타낼 수 있다.

 

3-D. Candidate Path Generation

앞서 생성된 궤적에서 충돌 혹은 차량의 움직임/동적 특성에 제한이 있을 수 있다.

시스템의 응답 시간을 향상시키기 위해 제약 조건을 충족하지 못하는 궤적 점검을 한다.

그후 남은 궤적은 다음 모듈에서 최적 경로의 후보로 제시된다.

 

점검의 주요 구성 요소인 충돌, 곡률 및 가속도를 각각 살펴보자.

1) Collision Check

원은 충돌 점검을 위한 bounding boxes로 사용된다.

충돌 점검을 통과하기 위한 조건은 아래와 같다.

(( (x_{ij} - x_{ob})^2+ (y_{ij} - y_{ob})^2< (r_{car} + r_{ob})^2 ))

즉, 궤적점과 장애물 간의 거리 < 차량과 장애물 각 경계 상자 반경의 합 : 충돌X

 

(xob, yob) : 주어진 장애물의 좌표

(xij, yij) : 궤적 점 궤적 i의 좌표

r_car : ego vehicle의 원 경계 상자의 반경

r_ob : 장애물의 원 경계 상자의 반경

2) Curvature Check

생성된 궤적에서 각 점의 곡률은

(( \kappa_x[ij] \leq \kappa_{max}, 0 \leq j \leq N ))

(( \kappa_{max} )) :  최대 허용 곡률

 N : 궤적 포인트의 수

3) Acceleration Check

생성된 궤적에서 각 점의 가속도는

(( a_x [ij] \leq a_{max},0 \leq j \leq N ))

((a_max)) : 최대 허용 가속도

 

3-E. Cost Fuction

 

1) Jerk Cost Fuction

 

2) Trajectory Safety Cost Function

 

3) Trajectory Anti-Deviation Cost Function

 

---

4. Simulation and Analysis

4-A. Influence of cost fuction on trajectory selection

4-B. Performance of proposed algorithm

1) straight road scenario

2) curvy road scenario

3) intersection scenario

4) U-shaped road scenario

---

참고 자료

https://velog.io/@legendre13/Optimal-Frenet-planning-algorithm

Optimal Frenet planning algorithm

 

https://tonylim.tistory.com/63

Optimal frenet planning algorithm

https://jhrobotics.tistory.com/57

[AD] Frenet Frame Planning Algorithm 설명