Application of hybrid A star

Practical Search Techniques in Path Planning for Autonomous Driving이라는 논문을 읽고 리뷰를 작성했다.
2007년 DARPA Urban challenge에서 Stanford의 레이싱 팀인 Junior가 주차 환경에서 성공적인 navigation을 한것에 대한 핵심방법으로 planning을 하는데 50-300ms 정도로 realtime으로 동작할 수 있었다고 하여 논문을 찾아보았다. 주된 공헌은 기존의 hybrid A star에서의 단점을 해결했다고 한다.

참조

hybrid A*

Reeds-Shepp model

Two heuristics

non-holonomic without obstacles

자동차 운동의 제약사항을 고려하고 장애물이 없다는 가정하에서 목표지점까지 경로를 생성

정의

state를 $$(x, y, \theta)$$ 로 정의

절차

1. global state 로 $$(x_g, y_g, \theta_g) = (0,0,0) $$ 으로 설정
2. 모든 point들 $$(x,y,\theta)$$ 에 대해서 장애물이 없다는 가정하에 목적지까지 Euclidian distance를 사용하여 shortest path를 계산

위의 과정을 통해서 목표지점에 가까운 노드들을 먼저 탐색하게 된다.

holonomic with obstacles

절차

차량의 운동학적 제약사항을 고려하지 않고 장애물이 있는 map에서 dynamic programming으로 목적지까지의 shortest path를 계산

Analytic expansion

이전의 A*알고리즘은 discrete state에서 동작하므로 resolution에 정확도가 의존적이므로 continuous state에서 정확도와 search speed를 높이기 위해서 Reed-Shepp model 기반의 analytic expansion을 통한 search를 시도했다. 다시쉽게 말하면 non-holonomic without obstacle heurtistic으로 Reed-Shepp model을 사용했다.

지도에 object 없다고 가정

Euclidean distance 대신에 사용

노드를 확장할 때 사용

Dubian path

어떤 point A부터 또 다른 point B까지 curve segment 혹은 line segment를 따라 foward 방향으로이동하는 경로이다. Dubins Curves

6가지의 가능한 최적의 경로가 있다. $${LRL, RLR, LSL, LSR, RSL, RSR}$$

Reeds-Shepp path

Dubian path를 따라 움직이지만 forward 와 reverse 방향으로 둘다 움직일 수 있는 경로이다.

48가지의 가능한 최적의 경로쌍이 있다.

만약 경로상에 장애물이 없다면 Reed-shepp 경로를 생성하고 chilren node를 추가시켰다.

계산상의 문제로 simple selection rule을 적용하여 N개의 노드마다 1개의 노드씩만 확장했다.

flowfield algorithm

single source shortest path를 찾는 A*알고리즘과 달리 all pair shortest path를 찾는 알고리즘이다. 장애물을 피하기 때문에 hybrid A*만큼이나 유용한 알고리즘이다.

Vornoi field

vornoi field는 가장 가까운 voronoi edge와 obstacle까지의 거리를 알려준다.

voronoi diagram

가장 가까운 장애물까지의 거리를 알려주며 경계또한 obstacle이다.

flowfield 알고리즘을 통해 생성할 수 있다.

절차

1. voronoi edge를 확인
2. voronoi edge부터 obstacle까지의 거리를 flowfield 알고리즘으로 찾음
3. voronoi field를 계산하는데 위에서 계산된 거리를 가지고 수식을 세워서 찾음

효과

가장 가까운 장애물과의 거리를 알 수 있기 때문에 collision detection 속도를 향상시킬 수 있다.

해야할 것

각 노드에 장애물까지의 거리에 따라 cost를 더함

Path-cost function using voronoi field

definition of Voronoi field

$$ \rho v(x,y) = \begin{cases} (\frac{\alpha}{\alpha + do(x,y)}) (\frac{dv(x,y)}{do(x,y) + dv(x,y)})\frac{(do-do^{max})^2}{(do^{max})^2} \in [0,1] & for\mbox{ } do \leq do^{max} \\0 & for \mbox{ } do > do^{max} \end{cases}$$

$$ do $$ = distance to the nearest obstacle

$$dv $$ = distance to the edge of the Generalized Voronoi Diagram(GVD)

$$ \alpha , do^{max} > 0$$ 는 field의 falloff rate (저하율) 과 maximum effective range를 조절하는 constant이다.

왜 다른 potential field 대신에 voronoi feild를 사용했는가?

탐색에 사용가능한 여유공간에서 비례하여 필드값을 조정할 수 있기 때문이다. 그래서 더 좁은 구간도 navigation이 가능하게 했다. 즉, 장애물들이 조밀하게 붙어있는 공간에서 필드의 범위를 좁게 조정가능하다. Naive potential field = $$ \rho (x,y) = \frac{\alpha}{\alpha + do(x,y)}$$ 의 경우는 좁은 구간에서 high potential을 갖는 단점이 있기 때문에 이를 개선하고자 voronoi field를 사용하게 되었다.

적용의 어려움과 실질적 적용

하지만 non holomic한 car의 경우 voronoi graph를 따라서 navigating하는 것이 불가능했다.

Navigation function 과 Laplace potential 은 voronoi field랑 비슷하다. 그것을 이용하면 local minima가 없는 potential function을 global navigation을 위해서 만들 수 있었다.

그러므로 볼록한 장애물이 있는 경우 local minima가 없는 potential function을 만들 수 있으므로 voronoi field가 매우 유용하다.

Local Optimization and Smoothing

Hybrid A* algorithm을 사용하면 path가 ugly하게 생성되므로 차가 따라가기 쉽도록 매끄럽게 만들어주는 과정이 필요하다. 이 논문에서는 gradient decent 방법을 사용했다고 한다. 간략히 설명하면 original path 중간 중간 point를 더 넣고 차가 따라가기 쉽도록 PID control을 하는 것이다.

문제는 hybrid A* algorithm을 사용할 때 차의 뒷바퀴를 사용한 것인데, 반대방향으로 path를 옮겨 줄 필요가 있다. 즉 mirrored path를 따라 차량이 뒤로 움직인다.

hybrid state A* solution에 다음과 같은 two-stage post processing을 했다고 한다.

1. non-linear optimization program
2. non-parametric interpolation

1. non-linear opitmization program

hybrid A* 알고리즘 수행결과 $$\textbf{x}_i = (x_i, y_i), i \in [1,N]$$ 의 vertex sequence를 얻는다.

Notation

$$ o_i $$ 는 현재의 vertex로부터 가장 가까운 obstacle의 위치

$$\Delta \textbf{x}_i = \textbf{x}_i - \textbf{x}_{i-1}$$ 각 vertex에서 displacement vector

$$\Delta \phi_i = |tan^{-1}\frac{\Delta y_{i+1}}{\Delta x_{i+1}} - tan^{-1}\frac{\Delta y_{i}}{\Delta x_{i}}|$$ 각 vertex에서 tangential angle의 변화량

$$\kappa_{max}$$ = 최대 허용가능한 path의 곡률

$$\sigma_o(.) , \sigma_k(.)$$ = penalty function으로 경험적으로 quadratic funciton을 사용

$$w_{\rho}, w_{o}, w_{\kappa}, w_{s}$$ = 각 objective term의 weight

objective function

첫번째 term은 좁거나 넓은 통로의 장애물을 효과적으로 피하도록 도와준다.

두번째 term은 물체들과의 충돌거리에 upperbound 를 두어 panalty를 부여한다.

세번째 term은 궤도의 instantaneous curvature에 upperbound를 두어서 자동차가 움직이기 쉽도록 non holonomic constraint를 강화한다.

네번째 term은 path자체가 smooth하게 만들어지도록 한다.

각 term의 gradient의 계산은 논문에 어떻게 구하는지와 식이 잘 나와있으므로 생략한다.

2. non-parametric interpolation using conjugate gradient

1번으로 구한 경로는 abrupt steering angle이 구해진다.

그래서 interpolation을 통해 좀더 smooth하게 다듬을 필요가 있다.

interpolation방법으로는 parametric방법과 non parametric 방법이 존재하는데

많은 parametric 방법은 input의 noise에 민감하여 input 정점들간의 간격이 매우 가까울 때 출력에서 큰 oscillation이 발생한다.

그래서 저자는 non-parametric interpolation방법을 차용했다.

장점

온라인으로 검색트리를 세우기 위해서 도달가능한 상태들을 미리계산하는 motion primitive를 사용했다. 즉

경로를 기하학을 고려하여 만들어 두어서 온라인으로 경로를 빠르게 세우는 것을 도와준다.

단점

1. 경로 유형의 즉각적인 변화를 가정했기 때문에 움직임이 부자연스럽다.
2. 장애물이 없는 상태에서 경로를 설정했기에 실제 동작시 장애물에 의해 움직임이 고착이 될 가능성이 있다.