자율주행 스터디 EM motion planner

https://arxiv.org/abs/1807.08048

Baidu Apollo EM Motion Planner

 

1980년부터 자율주행연구시작
로드 사망자를 줄이고, 트래픽 효율, 편한 여행을 제공하기 위해 연구된다
그러나 환경을 센싱하는데 정확도가 요구되며
각각 다른 시나리오에 안전운전에 대한 깊은 이해가 필요하다
아폴로 오픈소스를 구축하였다
HD- 맵 모듈은 고-definition 맵을 제공하며,
퍼셉션과 로컬라이제이션 모듈은 동적 환경 정보에 필요하다.
모션 플래닝 모듈은 모든 정보를 고려하여 안전하고 스무스한 경로를 생성한다.
안전이 최우선 과제이다
cycle time , efficiency 또한 중요하다.
range coverage 는 우리는 경로를 적어도 8초이거나 200미터의 모션 경로를 제공하는데 목표를 둔다
이유는 안전거리 확보 때문이다.

알고리즘의 execution time 도 중요한 문제이다(컴퓨터가 execute(load-fetch-execute) 하는데 걸리는 시간)
위급상황에서 300ms 의 인간반응속도에 비교, 시스템은 100 ms 안에서 반응할 수 있다.

북경근처 파크를 만들어 200대가 실험운행

 

https://youtu.be/Zcw70M51FbM

https://youtu.be/Zcw70M51FbM

https://youtu.be/opqtckENIs8

https://youtu.be/cwJi1xrpj7E

 

https://youtu.be/2Os-4TRCwFo

아폴로 겜엔진(unity와도 협업)

https://github.com/ApolloAuto/apollo

GitHub - ApolloAuto/apollo: An open autonomous driving platform

https://www.youtube.com/watch?v=1WSGlFK99q8

구글 지도--> 길 절단후 경로 지정 -->

차선은 perception 에서 탐지 안되므로 HDMAP 에서 추출함

네비게이터를 클라우드 서비스로 구현
두개의 데이터 베이스

인간의 운전 데이터를 업로드 할 수 있는 인터페이스가 있음
목적지를 요구할때--> Routing Request

1. 구글 맵 API 로 전달하고(라우팅 요청)
2. 라우팅프로세서가 해당 정보를 기반으로 네비게이션 라인 데이터베이스에서 탐색영역과 가장 일치하는 항목을 찾음

 

HD map 은 더 세밀하므로 도시주행과 같은 환경에 적합
mode 1 은 시골길이나, 고속도로 등 쉬운 환경시나리오
자율주행 4단계가 mode3 정도

local coordination 에 의해 perception 이 만들어지므로 로컬정보 불필요
모터는 정적매핑요소 사용 --> 로컬 정보 사용 (차량위치 를 네비게이션 라인에 투영하기위해 localization 정보를 요청합니다.)

길 이 잘 보여야 함(condition) 정도
HD map 의 장점이 보이는 부분
MOTION PLANNING 은 이러한 점들을 다 고려, 혼합 사용하는 것 같음
relative map 이라는 것을 HD map 의 대체? 로 쓰이는 것 같음

 

센서퓨전시 카메라는 옆쪽(lateral?) 거리를 측정하는데 쓰임, 라이다는 위아래로  움직임(Longitudinal)

(장애물과 충돌 안하게,테슬라도 귀 부분에 카메라가 있었음..기술부채와 관련된 내용인듯)

 

HD map 에서 정적인 요소들을 네비게이터(대형 트럭, 네비게이션 모드)

에 넣고 비젼 베이스 인식을 추가(동적인 장애물들, 리얼타임 맵 요소들)해서

Relative Map 을 만든다(여러 차선 지원,도시 주행...안전에 더 포커스) 

이것이 최신 동향이다.

 

 

---

 

2차원 --> 1차원 횡방향,종방향 으로 분해 

-----------------------------------------------------------------------------------------
https://www.youtube.com/watch?v=jiZhSIrmODk-----------------------------

navigation mode

perception 모듈로 planning trajectory 를 만들 수 있음

https://daobook.github.io/apollo/docs/specs/qp_spline_st_speed_optimizer.html

QP-Spline-ST-Speed Optimizer — Apollo Auto 0.0.1 文档

운전자의 운전경험도 중요하다.(comfort 제공)
레벨4의 자율주행에서 차선변경 전략은 필수적이다.
보편적 접근법중 하나는 비용함수를 모든 가능한 차선에 적용하여 제일 낮은 코스트를 가지는 후보를 찾아 final 경로를 찾는 것이다.
이런 서치알고리즘을 개발하는 것이다.
물론 문제점이 있다
1. 다수의 차선이 있는 서치 스페이스공간은 연산하기에 비싸다
2. 교통 규약(우측 통행, 신호등) 은 차선마다 다르다. 그래서 같은 프레임에 트래픽 규약을 적용하기 쉽지 않다
그리고 자전거로인한 갑작스러운 경로불안정 문제도 고려대상이다.

  (1) 시스템의 최상위 계층은 병렬로 계산된 차선 수준 궤적을 비교하여 차선 변경 시나리오를 처리하는 다중 차선 전략입니다. (2) 레인 레벨 궤적 생성기 내부에서 Frenet 프레임을 기반으로 경로 및 속도 최적화를 반복적으로 해결합니다.  

  경로 및 속도 최적화를 위해 동적 프로그래밍과 spline 기반 2차 프로그래밍의 조합을 제안하여 교통 규칙, 장애물 결정 및 부드러움을 동시에 처리하기 위해 확장 가능하고 조정하기 쉬운 프레임워크를 구성합니다. 플래너는 고속도로 및 저속 도시 운전 시나리오로 확장할 수 있습니다. 또한 시나리오 일러스트레이션과 온로드 테스트 결과를 통해 알고리즘을 시연합니다.  

b.
경로 속도 접근 방식은 동적 장애물의 출현으로 최적이 아닐 수 있습니다. 그러나 경로와 속도가 분리되어 있으므로

이 접근 방식은 경로 및 속도 최적화 모두에서 더 많은 유연성을 달성합니다.

EM 플래너는 경로와 속도를 반복적으로 최적화합니다.

마지막 사이클의 속도 프로파일은 경로 최적화 프로그램에서 다가오는 저속 동적 장애물과의 상호 작용을 추정하는 데 사용됩니다.

그런 다음 생성된 경로는 최적의 속도 프로파일을 평가하기 위해 속도 최적화기로 전송됩니다.

인풋 HD Map --> 라이다 포인트 들이 ROI 안팎에 있는지 판단

LIDAR 포인트들을 2D 양자화로 룩업 테이블로 쿼리화시키고 연산함

 

초기 값( 횡측 오프셋, 초기 포인트(계획된 경로의) ,가속도, 가속도의 미분) 

경로 최적화 과정 : 프라넷 프레임으로 프로젝트 하고 --> 부드러운 경로 생성 (EM) 

SL (경로) --> ST(속도)  연산으로 넘어간다 

# path - time graph = ST

 

ST 프로파일을 n 개의 세그먼트로 나눔

다항식으로 각 세그먼트 경로를 디파인할 수 있음

 

 

코스트 함수로 경로를 부드럽게 할 수 있음

 

첫 번째 E-스텝에서는 레인 Frenet 프레임에 장애물이 투사됩니다.

장애물 정보가 ST 그래프(경로-시간) 의 그리드들로 discretized 됩니다.

 

 

5차 다항식으로 세그먼트 경로들이 디파인됨

 

 

이 투영에는 정적 장애물 투영과 동적 장애물 투영이 모두 포함됩니다.
정적 장애물은 데카르트-프레네 프레임 변환을 기반으로 직접 투영됩니다.

Apollo 프레임워크에서 동적 장애물의 의도는 장애물 이동 궤적으로 설명됩니다.

이전 주기 계획 궤적을 고려하여 각 시점에서 추정된 동적 장애물 및 자아차 위치를 평가할 수 있습니다.

그러면 각 시점에서 dynamic 장애물과 ego 카의 중첩이 Frenet 프레임에 매핑됩니다.
https://fjp.at/posts/optimal-frenet/

Trajectory Planning in the Frenet Space

---

---

Frenet Frame

두 개의 M-단계에서 경로 및 속도 프로파일은 동적 계획법과 2차 계획법의 조합으로 생성됩니다.

SL 및 ST 프레임에 장애물을 투영했지만

최적의 경로 및 속도 솔루션은 여전히 볼록하지 않은 공간에 있습니다.

 

 

Frenet frame

 

그래서, 

우리는 2차 프로그래밍 기반 스플라인 옵티마이저에 대한 볼록 껍질을 결정하기 위해 대략적인 결정을 사용합니다.

 

그런 다음 옵티마이저는 볼록 껍질 내에서 솔루션을 찾을 수 있습니다. 다음에서 모듈을 다룰 것입니다.

https://youtu.be/j_jBK7zJ1vU

 

quadratic spline

convex hull

---

----------------------------------------------------------------------------------------------------------------------
----------------------------------------------------------------------------------------------------------------------

장애물과 자동차의상태는 x,y,theta 에 해당하는 직각좌표계에서 Frenet frame 좌표계로 맵핑된다.
현재장소, 측면, 측면의 미분들로 이루어지는 좌표계이다.
정적 장애물의 위치는 시간에 따라 변하지 않으므로 매핑은 간단합니다.

동적 장애물의 경우 ego 자동차의 마지막 주기 궤적을 사용하여 장애물을 매핑했습니다.
마지막 주기의 이동 궤적을 Frenet 프레임에 투영하여 스테이션 방향 속도 프로파일을 추출합니다.
이것은 특정 시간이 주어진 ego 자동차의 스테이션 좌표의 추정치를 제공합니다.
추정된 ego 자동차 스테이션 좌표는 동적 장애물 상호 작용을 평가하는 데 도움이 됩니다.
----------------------------------------------------------------------------------------------------------------------

자아 자동차의 스테이션 좌표가 동시에 장애물 궤적 지점과 상호 작용하면 SL 맵의 음영 영역(그림자 진 영역)
이 동적 장애물과의 예상 상호 작용으로 표시됩니다. 여기서 인터랙션은 에고카와 장애물 경계 상자가 겹치는 것으로 정의됩니다. 예를 들어, 그림 4와 같이 다가오는 동적 장애물과 예측 모듈에서 추정된 해당 궤적은 빨간색으로 표시됩니다.

다가오고 있는 빨간색 동적 장애물

자아 자동차는 파란색으로 표시됩니다. 다가오는 동적 장애물의 궤적은 먼저 시간에 따라 여러 궤적 지점으로 이산화된 다음, 그 지점이 Frenet 프레임에 투영됩니다. 자아 자동차의 스테이션 좌표가 투영된 장애물 지점과 상호 작용한다는 것을 알게 되면 겹치는 영역(그림에서 보라색으로 표시)이 Frenet 프레임에 표시됩니다. ST 투영은 자아 자동차의 속도 프로필을 평가하는 데 도움이 됩니다. 경로 최적화 프로그램이 Frenet Frame에서 정적 장애물과 동적 장애물 궤적 모두에서 부드러운 경로 프로필을 생성한 후
상호 작용이 있는 경우 경로에 투영됩니다. 상호 작용은 경계 상자가 겹치는 것으로도 정의됩니다. Fig. 5에서 빨간색으로 표시된 바와 같이 전방 2초 및 40미터 지점에서 자아주행 경로로 절단된 하나의 장애물과 자아차 뒤에 있는 하나의 장애물이 녹색으로 표시되어 있다. 나머지 영역은 속도 프로파일 가능 영역입니다. 속도 최적화 M-step은 이 영역에서 실현 가능한 부드러운 솔루션을 찾으려고 시도합니다.
빨: 장애물이 끼어들기 해서 안전거리랑 겹치면 빨간색
나머지 영역은 실현 가능한 속도 영역

 

----------------------------------------------------------------------------------------------------------------------

-----------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------

동적 프로그래밍 경로에서는 선택된 경로를 기반으로 실현 가능한 터널(feasible tunnel) 이 생성됩니다.

M-step 경로 최적화 프로그램은 Frenet 프레임의 경로 프로파일을 최적화합니다. 이것은 횡좌표 l = f(s) w.r.t의 최적 함수를 찾는 것으로 표현됩니다. 볼록하지 않은 SL 공간의 스테이션 좌표(예: 왼쪽과 오른쪽에서 조금씩 이동하면 두 개의 로컬 최적이 될 수 있음)
---------------------------------------------------------------------------------------------------------------------
전형적으로 이러한 전략은 비수동적 수동적 차선변경 시나리오를 커버할 수 있다.
EM 플래너에서 비수동적 레인 변경은 최종 목적지에 도달하기 위해 라우팅 모듈에 의해 트리거되는 요청입니다.
다이나믹 프로그래밍 경로 단계 (Dynamic Programming)
는 실현 가능한 터널 및 장애물 넛지 결정과 함께 대략적인 경로 프로파일을 제공합니다.
도 6에 도시된 바와 같이, 단계는 격자 샘플러, 비용 함수 및 동적 프로그래밍 검색을 포함한다.

격자 샘플러는 Frenet 프레임을 기반 으로 합니다.
도 7에 도시된 바와 같이, 다중 행의 포인트는 먼저 자아 차량보다 앞서 샘플링된다. (path lattice sampling)
서로 다른 행 사이의 점은 5차 다항식 모서리로 매끄럽게 연결됩니다.
점 열 사이의 간격 거리는 속도, 도로 구조, 차선 변경 등에 따라 다릅니다.
프레임워크를 사용하면 애플리케이션 시나리오를 기반으로 샘플링 전략을 사용자 지정할 수 있습니다.
예를 들어, 차선 변경은 현재 차선 주행보다 더 긴 샘플링 간격이 필요할 수 있습니다. 또한, 격자 총 스테이션 거리는 안전 고려 사항을 위해 최소 8초 또는 200미터를 포함합니다.
격자 가 구성된 후 각 그래프 가장자리는 cost 함수의 합산으로 평가됩니다.
우리는 SL 투영법, 교통 법규 및 차량 역학의 정보를 사용하여 기능을 구성합니다.
총 에지 cost 함수는 부드러움, 장애물 회피 및 차선 비용 함수의 선형 조합입니다.

평활도 비용 함수에서 f' (s) 는 차선과 자아차 사이의 방향 차이를 나타내고
f '' (s) 는 경로의 곡률과 관련되며 f''' (s) 는 곡률의 미분과 관련됩니다.

다항식의 형태로 위의 비용도 분석적으로 평가할 수 있습니다. 에지가 주어진 장애물 비용은 모든 장애물과 함께 일련의 고정 스테이션 좌표 {s0, s1, ..., sn}에서 평가됩니다. 장애물 비용 함수는 장애물과 자아 자동차 사이의 경계 상자 거리를 기반으로 합니다. 거리를 d로 표시합니다. 개별 비용의 형태는 다음과 같습니다.

여기서 Cnudge는 단조 감소 함수로 정의됩니다. dc는 안전을 위해 버퍼를 남겨두도록 설정됩니다. 넛지 범위 dn은 시나리오에 따라 조정 가능합니다. Ccollision은 충돌 비용으로 실행 불가능한 경로를 감지하는 데 도움이 되는 큰 값을 갖습니다. 차선 비용은 안내선 비용과 도로 비용의 두 부분으로 구성됩니다. 안내선은 주변 장애물이 없을 때 이상적인 주행 경로로 정의됩니다. 이 선은 일반적으로 경로의 중심선으로 추출됩니다. 안내선 함수를 g(s)로 정의합니다. 그러면 다음과 같이 측정된다.

----------------------------------------------------------------------------------------------------------------------
----------------------------------------------------------------------------------------------------------------------

스플라인 QP 경로 단계 는 동적 프로그래밍 경로 단계를 개선한 것입니다.
동적 프로그래밍 경로에서는 선택된 경로를 기반으로 실현 가능한 터널(feasible tunnel) 이 생성됩니다.
그런 다음 스플라인 기반 QP 단계는 그림 8 과 같이 이 가능한 터널(feasible tunnel) 내에서 부드러운 경로를 생성합니다.

The spline QP path is generated by optimizing an objective function with a linearized constraint through the QP spline solver. Fig. 9 shows the pipeline of the QP path step. The objective function of the QP path is a linear combination of smoothness costs and guidance line cost. The guidance line in this step is the DP path. The guidance line provides an estimate of the obstacle nudging distance. Mathematically, the QP path step optimizes the following functional:

In addition to the boundary constraint, the generated path shall match the ego car’s initial lateral position and derivatives
긁히지 않기 위해서
초기 측면 포지션 = f(s_0)
--> 미분: 곡률 --> 두번 미분: 곡률의 미분 을 고려하여 생성된 경로가 매치되어야 한다

----------------------------------------------------------------------------------------------------------------
----------------------------------------------------------------------------------------------------------------

경로 옵티마이저에서와 유사하게 ST 그래프에서 최상의 속도 프로파일을 찾는 것은 볼록하지 않은 최적화 문제입니다.

스피드 옵티마이저는 ST 그래프(아까Franet frame 좌표계에 맵핑한) 에 스피드 프로파일을 생성해 냅니다
ST 그래프는 S(t) 에 대한 station 함수입니다

최단거리를 찾는 방법

g(s) 는 DP 경로 결과이다.
QP 경로의 제약 조건에는 경계 제약 조건과 동적 실행 가능성이 포함됩니다. 이러한 제약 조건은 스테이션 좌표 s0, s1, ...., sn의 시퀀스에서 f(s), f'(s) 및 f ''(s)에 적용됩니다. 경계 제약 조건을 추출하기 위해 스테이션 포인트에서 실행 가능한 범위가 추출됩니다. 각 지점의 실행 가능한 범위는 (low,i, ≤ lhigh,i)로 설명됩니다. EM 플래너에서 자아 차량은 자전거 모델로 간주됩니다. 따라서 자아 자동차의 방향도 중요하기 때문에 단순히 l = f(s)에 대한 범위를 제공하는 것만으로는 충분하지 않습니다.

볼록 및 선형 변형, 우리는 ego 자동차의 전면과 후면 끝에 두 개의 반원을 추가합니다. 전후 휠 중심 거리를 lf로, 차량 폭을 w로 표시합니다. 그러면 왼쪽 앞 모서리의 측면 위치는 다음과 같이 지정됩니다.

l = f(s) 는 횡좌표

 

---

DP speed optimizer 에 대한 설명 

첫 번째 항은 속도(Velocity, 거리 S 를 한번 미분하면 속도)  유지 비용입니다.

이 term은  장애물이나 신호등 제한이 없을 때 차량이 지정된 속도를 따라야 함을 나타냅니다.

Vref는 도로 속도 제한, 곡률 및 기타 교통 규정에 의해 결정되는 기준 속도를 설명합니다.

g 함수는 Vref보다 작거나 큰 값에 대해 서로 다른 패널티를 갖도록 설계되었습니다.

가속도 및 jerk(가속도의 시간미분) 제곱 적분은 속도 프로파일의 부드러움(smoothness) 정도을 설명합니다.

마지막 용어인 C_obs는 총 장애물 비용을 설명합니다.
모든 장애물까지의 ego 자동차의 거리가 평가되어 총 장애물(obstacle) cost이 결정됩니다.

동적 프로그래밍 검색 공간도 차량 동적 제약 조건 내에 있습니다.

동적 제약 조건에는 가속도, 저크 한계 및 단조성 제약 조건이 포함됩니다.

생성된 궤적이 도로에서 주행할 때 후진 기동을 수행하지 않아야 하기 때문입니다.

후진은 주차 또는 기타 지정된 시나리오에서만 수행할 수 있습니다.

검색 알고리즘은 간단합니다.

프로세스를 가속화하기 위해 차량의 동적 제약 조건을 기반으로 하는 일부 필요한 가지치기도 적용됩니다. 검색 부분은 자세히 다루지 않겠습니다.

-------------------------------------------------------------------------------------------------------------------
-------------------------------------------------------------------------------------------------------------------

 

the spline QP speed step includes three parts: cost functional, linearized constraint and spline QP solver.

초기 속도와 가속도를 일치시키기 위한 제약 조건도 있습니다. 첫 번째 제약 조건은 지정된 지점에서 평가되는 단조성입니다. 두 번째, 세 번째, 네 번째 제약 조건은 교통 규제 및 차량 동적 제약 조건의 요구 사항입니다. 비용 목표와 제약 조건을 정리한 후 스플라인 솔버는 그림 14와 같이 부드럽고 실현 가능한 속도 프로필을 생성합니다. 경로 프로필과 결합하여 EM 플래너는 제어 모듈에 대한 부드러운 궤적을 생성합니다.