A generic driving strategy for urban environments

Reference
Constantin Hubmann, A Generic Driving Strategy for Urban Environments, ITSC, 2016

A generic driving strategy for urban environments

Introduction

이 논문은 다양한 장애물이 존재하는 도심환경에서의 쾌적성, 교통 법규, 동역학적 제약을 동시에 고려한 종방향 주행전략을 세우는 문제를 다루고 있다. 특정 한 상황에만 적용가능한 알고리즘이 아닌 어떤 상황에서도 적용가능한 알고리즘을 설계한다. 장애물들을 static events (신호등, 보행자 등) 와 dynamic events (차선 합류하는 자동차 등) 이 두가지로 표현하고, rule base가 아닌 A* 알고리즘을 활용하여 내 차량의 종방향 주행전략을 세우게 된다.
이 알고리즘은 13s 동안의 계획 을 하는데 최악의 경우 80ms 만에 찾을 수 있게된다.

장애물의 종류 Static Events traffic sign crossing pedestrian Dynamic Events NPC vehicles

Problem Definition and Goal

이 알고리즘은 짧은 궤적을 계획하는 것이 아닌, 10s 이상의 장기 주행 전략을 세우게 된다. 교통 범규, 도로 곡률, 다양한 이벤트들을 동시에 고려하는 global decision making problem으로, 여러 local minima 중에 global minimum을 찾아야하는 non-convex 문제이다. 계획을 위해 A* 기반 discrete 탐색을 수행하게 된다. 이후 결과를 바탕으로 앞에 3초를 local trajectory planning 에 사용하게된다.

Driving Strategy	→	Trajectory Planning
Decision Problem		planning problem in homotopy
Non-Convex		convex problem
$t_{hor} \geq 10s$		$t_{hor} \sim 3s$

Algorithm

• state: $x = [s, v, t]^t$
• set of actions: $[-2, -1, 0, 1]$
• solved by A* search

transition model

The discretized state transition model :
\(x_{i+1} = \begin{bmatrix} s_{i+1}\\ v_{i+1}\\ t_{i+1} \end{bmatrix} = \begin{bmatrix} 1 & \Delta t & 0 & 0 \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & \Delta t \end{bmatrix} \begin{bmatrix} s_{i}\\ v_{i}\\ t_{i}\\ 1 \end{bmatrix} + \begin{bmatrix} \frac12 (\Delta t)^2\\ \Delta t\\ 0 \end{bmatrix} a_i\)

action a 에 따라 상태가 이동하게 된다.

cost functional

각 step별 cost계산은 다음과 같이 이루어진다.
\(c(x_i, a, x_{i+1}, E) = c_V(x_{i+1}) + c_A(a) + c_E(x_i, x_{i+1}, E)\)

• $c_V(x_{i+1})$ : cost for any deviation to the desired speed
  • $v_{des} < v_{x_{i+1}}$ : $(v_{des} - v_{x_{i+1}})^2$
  • $v_{des} == v_{x_{i+1}}$ : 0
  • $v_{des}$ > $v_{x_{i+1}}$ : $(v_{des} - v_{x_{i+1}}) / 2$
• $c_A(a)$ : cost for taking action a
• $c_E(x_i, x_{i+1}, E)$ : cost for a collision while traversing from $x_i$ to $x_{i+1}$
  • $c_E = c_{Es} + c_{Ed}$ : 정적 장애물 + 동적 장애물
  • 장애물 충돌시: cost = inf
  • $c_{Ed}$ (동적 장애물) 같은 경우 following distance 만큼 cost map을 만들어 주게 되는데
    차에 가까워 질수록 cost가 높아지도록 map을 설정 하였다.

A* search 를 이용한 planning

heuristic 같은 경우 논문에서는 ICS를 활용하여 사용하였지만
나는 구현의 어려움이 있어 heuristic function을 사용하지 않았다.
나의 구현은 사실상 다익스트라로 한 방식이다.
코드 바로가기

discrete하게 A* search을 수행하여 driving strategy를 계획하게 된다.
아래는 간략한 A*에 대한 수도 코드이다. A*에 대해 자세히 보러가기

open_set = dict()
closed_set = dict()

open_set[start_node_idx] = start_node
while len(open_set):
    curr = open_set에서 cost (g + h) 가 가장 작은 node 선택

    del open_set[curr_id]
    closed_set[curr_id] = curr

    if curr is goal_node:
        break

    for action in action_set:
        neighbor_node = 선택된 action를 이용해 다음 확장할 노드 생성
        if closed_set에서 현재 노드가 이미 존재하는 것 보다 cost 더 작은지 확인:
            작으면 삭제
        if open_set에서 현재 노드가 이미 존재하는 것 보다 cost 더 작은지 확인:
            작으면 삭제
        if open_set에도 없고 closed_set에도 없으면 탐색 추가:
            open_set[neighbor_id] = neighbor_node

Result

13s planning을 100ms 마다 현재 이벤트를 반영하여 시뮬레이션을 진행하고 있다.

• 왼쪽: 현재 내 자동차 상황에서 장애물들의 상대위치와 그 위치로 계획된 결과를 보여준다.
• 오른쪽: planning된 결과로 시뮬레이션을 보여준다.

시뮬레이션 결과 영상