2022 Tesla AI day FSD Planning 정리 [작성 중]

레퍼런스 : https://www.youtube.com/watch?v=ODSJsviD_SU&t=10562s 

Tesla AI Day 2022

테슬라 2022 AI Day 프레젠테이션 중 FSD - Planning 내용 정리

 

정리 이유

• 테슬라 FSD의 Planning 아키텍처 벤치마킹 하기 위함
• 테슬라는 차량 OEM 중 최고 수준의 자율주행 기술을 양산 판매하고 있음
• 복잡한 도로 상황이 연속될 때 자연스러운 Planning의 대응 및 전환 가능한 아키텍처를 벤치마킹 하기 위함

 

0. Planning 아키텍처 입력 신호

• FSD (Full Self Driving) 아키텍처 내 Planning 모듈은 Neural Network로부터 Lanes, Occupancy, Moving Objects 신호를 입력받는다. 

1. Joint Trajectory Planning : Ego Vehicle과 Object의 State를 통합해 최적 Trajectory 도출

Trajectory Presentations

위 Notation은 FSD Planning에서 정의한 Trajectory이다. Trajectory를 Time step(0 ~ T) 별 s (State)와 a (Action)으로 정의하였다. s와 a 는 최적화 관점에서 보면 State와 Input과 동일하다. 위 정의에서 인상적인 것은 Trajectory State의 Input으로  Longitudinal & Lateral Jerk를 정의한 것이다. 보통은 종가속도와 Wheel Steering을 Input으로 설정할텐데 말이다. (차량 모델에서 입력이 종가속도와 Wheel Steering이므로) 또한 State에 종/횡 Dynamic State를 통합해 설정하였다. 즉, 테슬라의 FSD Planning은 종방향과 횡방향을 분리해 계산하지 않고, 한번에 고려해 Trajectory를 생성하는 형태이다.

 

장점

1. 종/횡 State를 통합해 Trajectory를 정의하였기 때문에, 종/횡을 고려한 Action (차량의 거동)을 도출할 수 있다.
2. 위 정의는 종/횡 가속도 State에 Constraint를 추가하기 용이하다.

궁금한 점

1. 최적화 문제를 풀기 위해선 State간 관계를 Constraint로 설정해야 한다. 최적화 문제를 Convex로 구성하기 위해선 Constraint는 Linear 해야 한다. 그말인 즉슨, State간 비선형 관계를 선형으로 근사해야 함을 의미한다. 속도와 가속도 State간 관계는 선형이어서 문제 없다. 그런데 (x, y, theta)_t 와 (x, y, theta)_t+1 간 관계는 차량의 Non-holonomic 특성으로 비선형이기 때문에 Linearization이 필요하다. Tesla에서 이를 어떻게 대응했을지 의문이다. (Linearization 했을지?, Non-linear 최적화 문제를 풀었을 지? Linearization하고 오차 보상 텀을 추가했을지?)
2. Trajectory는 주변 객체 및 도로 구조물과 충돌하면 안된다. 충돌 제한 조건을 만족하는 Trajectory를 생성하기 위해선 State의 Constraint로 추가해줄 수 있다. 하지만 이는 Frenet Error Term으로 State를 정의했을 때 간단하게 설정 가능하다. (Frenet 좌표계에서 횡방향으로 1차원만 고려하면 되기 때문). Cartesian 좌표계에서는 충돌 제한을 위한 x, y Constraint 설정이 불가능하다. FSD Planning 아키텍처에서는
3. Trajectory Planning 과 Control 단 사이의 과정. 일반적으로 자율주행 Planning은 Control과 구분되어 있다. Planning단에서 속도 및 경로 프로파일을 출력하면 Control 단에서 PID 제어 / Stanley 제어 / Pure pursuit 제어 등을 사용해 추종한다. 혹은 최적화 시 Input을 가속도와 Wheel Steering으로 설정하여 해를 찾은 후, 이를 Low 제어 레벨로 출력해주기도 한다. 테슬라의 경우 종/횡 Jerk를 Input으로 계산하는데, 이 값으로부터 Control 단에서 실제 차량 제어까지 어떤 알고리즘으로 이루어지는지 확인이 불가능하다. 
4. Goal Time Step T를 어떻게 결정했을 까?

Costs and Constraints

Cost Term : g(tau), h(tau) = Square_Dist (Goal, Terminal State) & Longi/Lat Jerk  -> Square Term 인것 보니 QP 형태의 최적화인 것 같다. 마지막 상태 값 수렴을 위해 g(tau) 조건이 들어간 것으로 보인다. Smooth와 Efficiency를 위해 종/횡 Jerk를 Cost로 설정한것으로 보인다.

 

Constraint : D(tau1, tau2)는 Ego vehicle과 Object간 안전을 위한 여유거리 확보를 위해 설정한 제약 조건으로 보인다. C(tau1, tau2)는 각각 주행 Object에게 양보할 건지 먼저 지나갈 것인지를 판단하는 조건으로 보인다. (Constraint에 대한 이해가 부족하다..)

Joint Trajectory Planning for Ego and All other Agents

Tesla Planning 에서는 Cost Term에 Ego vehicle 뿐만 아니라 Object의 ocst도 고려한다. 도로 위 고려해야할 객체를 Cost Term에 넣어 최적화 함으로서 Joint Trajectory Planning라고 지칭한다.  제약 함수로는 Ego Vehicle 및 객체의 궤적간 안전 거리가 확보되어야 한다. 그리고 early, late case에 따른 제약 함수를 만족해야 한다.

 

 

Joint Planning은 매 Interaction마다 10ms 정도가 소요된다고 한다. 그래서 고려해야할 객체가 많은 비보호 좌회전의 경우 20개 이상의 객체를 고려해야 하고 100개 이상의 interaction 조합을 고려해야 한다. 즉, 연산 시간이 너무 많이 소요된다. (Tesla는 Planning의 주기를 50ms로 설정하였다.) 이를 Real Time에 적용하려면 어떻게 해야 할까?

 

 

2. Interaction Search