0. Title
- Baidu Apollo EM Motion Planner (2018)
1. Authors
- Haoyang Fan
2. Abstract
- 안전성, 편안함, 확장가능성을 고려하여 Level 4 경로 계획을 다루는데 집중함
- 다차선 및 단차선 자율주행을 계층적으로 다룸
- 고속도로 및 저속 주행 시나리오 모두 확장 가능
3. Motivation
- 기존의 방식의 문제점인 연산량이 많고 교통법규를 일반화하기 힘들며(차선의 종류마다 다른 교통 법규) 경로 안정성을 고려하지 못한다는 문제점을 해결하고자 함
- Non-convex 문제를 Convex 문제로 변환하여 최적의 솔루션을 찾고자 하였음
- 기존의 방식의 연산량 문제를 해결하고자 함
4. Contributions(Findings)
- 기존의 방식의 문제점인 연산량이 많고 교통법규를 일반화하기 힘들며(차선의 종류마다 다른 교통 법규) 경로 안정성을 고려하지 못한다는 문제점을 해결함
- DP와 QP 각각 모두 non-convex domain에서의 한계가 있었다. 이를 DP + QP를 통해 해결하였다.
5. Methodology
[EM Planner의 구조]
1. Data Center에서 Planning하는데 쓰이는 모든 정보들이 모은다.
2. 정보들이 모이면 교통 규범과 장애물을 고려하여 차선수준의 Reference Lane 후보들을 생성한다. (HD Map과 Routing 모듈으로부터의 Global Path 기반)
3. 만들어진 Reference Lane 기반으로 Frenet frame을 구성한다.
4. 경로 최적화 단계에서는 주변 환경에 대한 정보(Static, Dynamic Obstacle)가 Frenet Frame으로 투영된다.(E-step)
5. 정보들이 투영된 Frenet Frame에서 스무스한 경로를 계획한다.(M-step)
6. 스무스한 경로가 계획되면 주변 장애물들을 station-time graph에 투영한다.(E-step)
7. 스무스한 Speed Profiles을 생성한다.
8. Path와 Speed Profiles을 결합하여 스무스한 Trajectory를 얻는다.
9. 모든 차선 수준의 Best Trajectories들을 Reference Line Trajectory Decider에 보내고
현재 차량의 State, Regulations, 각 경로들의 Cost를 통해 최종 경로를 결정한다.
Optimizer 부분의 세부 내용은 다음과 같다.
첫 번째 E-step에서 Static Obstacle은 Frenet Frame에 바로 투영되고 Dynamic Obstacle은 Obstacle의 경로에 의해 장애물의 의도가 묘사된다. 이전 사이클 경로를 통해 동적 장애물과 자율주행 차량의 위치를 각 시점에서 평가할 수 있고 이를 통해 동적 장애물과 차량의 겹침이 Frenet Frame 상에서 투영된다. 해당 알고리즘에서는 안전을 위해 동적 장애물의 투영은 저속 교통 및 다가오는 장애물만 고려하며 고속 교통에 경우 EM planner의 병렬 차선 변경 전략이 다룰 것이다.
두 번째 E-step에서 고속, 저속 및 다가오는 장애물을 포함한 모든 장애물은 생성된 경로 프로파일을 기반으로 Stateion-time 프레임에서 평가된다. 장애물 궤적이 계획된 경로가 겹치는 경우, Station-time 프레임에서 해당 영역이 생성된다.
M-step에서는 DP와 QP의 결합으로 Path와 Speed Profiles이 생성된다.먼저 DP를 통해 Rough Solution을 얻고 QP-Based Spline Optimizer로 세밀하고 스무스한 Solution을 얻는다.
A. SL and ST Mapping (E-step)
SL Projection은 G2 smooth reference line 기반이다.
(x,y,$/theta$) 그리고 ($k$, $dk$) 형태로 장애물과 Ego 차량의 상태를 묘사하고 이를 Frenet Frame으로 맵핑한다.
1) Static 장애물은 Time Invariant므로 바로 투영한다 2) Dynamic 장애물은 마지막 사이클의 이동 궤적을 Frenet 프레임에 투영하여 스테이션 방향 속도 프로파일을 추출한다. 장애물 예측 경로와 Ego 차량이 겹치는 곳은 음영으로 표시하며 이러한 ST 투영은 Ego 차량의 Speed Profiles을 평가하는데 쓰인다.
B. M-Step DP Path
DP Step에서는 Lattic Sampler, Cost Function, DP Search를 이용하여 Rough Path Profiles, Feasible tunnels가 나온다.
Lattice Sampler는 Frenet Frame 기반이고 여러 행의 점이 Ego 차량 앞에 샘플링된다. 서로 다른 행 사이의 점은 5차 다항식에 의해 부드럽게 연결된다. 점 행 사이의 간격은 속도, 도로 구조, 차선 변경 등에 따라 달라진다. 차선 변경은 현재 차선 주행보다 더 긴 샘플링 간격이 필요할 수 있다. 또한, 총 Lattice Station 거리는 안전을 고려하여 최소 8초 또는 200m를 커버할 것이다. Lattice가 생성되고 각 Graph edge는 Cost function에 의해 평가된다.
f(s)는 Path Profile
f`(s)는 차량과 차선의 Heading 차이
f``(s)는 curvature
f```(s)는 derivative of the curvature
$d_c$는 안전 간격?
$d_n$는 협상할 수 있는 범위
다음 Cost들을 이용하여 DP Search를 통해 Cost가 가장 작은 후보를 선택한다.
C. M-Step Spline QP Path
앞서 선택된 DP Path와 Smooth Funcional을 통해 스무스한 경로를 만들고 Feasible Tunnel과 Vehicle Dynamics를 고려해 거동가능한 경로를 만든다.
여기서 g(s)는 DP Path고 f`(s)는 Heading, f``(s)는 curvature, f```(s)는derivative of curvature이다.
Boundary constraint가 convex, linear를 유지하기 위해 ego 차량에 두 반원을 추가한다. front-ro-rear distance는 $l_f$로 표기하고 차량의 너비는 $w$로 표기한다. 그러면 차량의 좌측 위치를 다음과 같이 표현할 수 있다.
$\theta$는 차량과 차선의 heading 차이이고 다음과 같은 수식을 통해 선형화할 수 있다.
f''(s)와 f''(s)의 범위 제한은 곡률 및 곡률 도함수와 관련이 있기 때문에 동적 실현 가능성으로 사용될 수 있다. 경계 제약 조건(앞에꺼) 외에도, 생성된 경로는 ego 차량의 초기 횡방향 위치와 (f'(s0), f'(s0), f''(s0))과 일치해야 한다.
모든 제약 조건이 스플라인 매개 변수에 대해 선형이기 때문에 QP Solver를 사용하여 문제를 매우 빠르게 해결할 수 있다.
D. M-Step DP Speed Optimizer
ST graph의 speed Profile을 생성한다.
속도, 가속도, Jerk는 다음과 같은 수식을 이용하여 추정한다.
Cost Function은 다음과 같다. Reference 속도(도로의 속도 제한)와 ego 속도의 차이, 가속도의 제곱, Jerk의 제곱, 장애물에 대한 Cost(장애물과의 거리)로 이루어져 있다.
다음과 같은 Cost Function을 통해 Cost가 가장 작은 스무스한 Speed Profile을 생성해낸다.
E. M-Step QP Speed Optimizer
DP에서 생성된 Rough Speed Profile과 Smooth Functional을 통해 스무스한 Speed Profile을 만들어내고 Linearized Constarint를 통해 실현가능한 Profile을 만들어낼 수 있게 된다.
Cost Function은 다음과 같다.(Speed Ref는 도로 제한속도이다.)
QP Speed Profile은 다음과 같은 제약 조건을 만족해야만 한다.
1. 무조건 차는 앞으로 가거나 정지해야함
2. 각 시점의 위치는 upper와 lower 사이에 있어야 함
3. 최대 속력보다 작아야 함
4. 최대 가속과 감속 사이에 있어야 함
5. 최대 Jerk의 음과 양 사이에 있어야 함
F. Notes on Solving Quadratic Programming Problems
경로 및 속도 최적화를 진행할 때 5차 다항식이면 충분하다는 결론이 있었고 QP 문제는 평균 3ms 이내에 해결될 수 있으며 이는 Real time에 적합하다
G. Notes on Non-convex Optimization With DP and QP
DP와 QP 각각 모두 non-convex domain에서의 한계가 있었다. 이를 DP + QP를 통해 해결하였다.
DP는 처리 시간의 제한으로 인해 샘플링된 후보들의 수는 샘플링 그리드에 의해 제한된다. 유한 그리드 내에서 최적화는 대략적인 DP 솔루션을 산출한다. 다시 말해, DP가 반드시 최적의 솔루션을 제공하는 것은 아니며, 거의 모든 경우에서 최적이 아니다.
QP는 Convex 도메인을 기반으로 솔루션을 생성한다. DP 단계의 도움 없이는 사용할 수 없으며. 무작위 또는 규칙 기반 결정은 QP가 실패하거나 로컬 최소값에 수렴하게 한다.
DP + QP는 DP를 통해 rough Path를 만들고 도메인을 Convex하게 바꿔준다. 그리고 QP는 Global Optima를 포함할 가능성이 높은 Convex region에서 최적의 경로를 찾아낸다.
6. Measurements
- 연산 속도
7. Limitations(If it's not written, think about it)
- 다른 MPC에 비해 주행 효율성이 떨어질 것으로 예상됨
8. Potential Gap
- 연산량이 많은 RL을 High Level 판단에 쓰고 다음과 같이 QP+DP를 Low level 판단에 쓰면 꽤 좋을지도?
[추가]
다차선 전략은 Non패시브와 패시브 차선변경 시나리오를 둘 다 다룬다.
Non패시브 차선변경은 차량이 목적지에 도달하기 위한 Routing 모듈에 의해 이루어진다.
패시브 차선변경은 기존 차선이 환경에 의해 막혔을 때 기동되는 차선변경
안전하고 편안하고 높은 성공률을 위해 해당 알고리즘은 두 가지를 병렬적으로 다룬다
Lattice search, 경로 샘플링을 통한 Path-Speed 최적화는 최적의 경로를 찾기에 매우 많은 연산량을 요구한다. 이를 해결하기 위해 Path 최적화와 Speed 최적화를 구분하여 iterative하게 최적화하고 QP, DP로 구분하여 최적화하여 QP 단계에서 Convex한 문제를 풀도록 바꾸어 Suboptimal하지만 Flexibility한 경로 계획을 할 수 있게 된다.
Quadratic-programming-based smoothing spline solver는 판단을 따르는 스무스한 경로와 Speed profiles를 만드는데 자주 쓰인다.
대부분의 SOTA는 무거운 결정 기반 알고리즘이지만 EM Planner는 light한 판단 기반 경로 계획법이다. 무거운 판단 계획법은 쉽게 이해할 수 있고 설명가능하지만 Real time 도메인에 적합하지 않을 수 있다.(연산량으로 인해)
