Gaussian process path

Introduction

 Along with recent dramatic advances in computing and sensing technology, robot applications have spread out from laboratories to real-world environment.

 One representative example would be a Google driverless car and the essence behind this work is a path planning algorithm. 

일반적인 path planning은 로봇의 현재 위치를 시작으로 하고, 목표점을 따라가는 경로를 찾는 문제로 구현될 수 있다. 만약 정적인 상황을 가정한다면, 이러한 경로와 해당하는 제어 신호를 offline에서 얻을 수 있고, 시간과 연산량에 상대적으로 덜 제약을 받는다. 하지만 동적인 상황에서는 feasible configuration space가 시간에 따라 변하기 때문에 현재 상황을 sensing 했을 때부터 path generation까지 시간 제약이 있다. 게다가 mobile robot의 경우 mobility가 있어야 하므로 연산량에 제약이 있다. 이러한 문제를 해결하기 위해서 가능한 모든 path의 set이 아닌 미리 정해진, 혹은 정해진 수의 subset만을 고려한 sub optimal path를 찾는 것을 목적으로 하곤 한다. 

이러한 subset of paths를 찾는 것에 대한 많은 연구가 진행되어 왔다. 특히 실제 dynamic environment에서 돌아가는 무인 자동차나, 무인 비행기 등에 이러한 연구가 진행되어 왔다. 이러한 접근은 Path planning의 optimization space를 줄이는 것 뿐만 아니라, dynamics가 복잡한 로봇에 있어서 미리 해당 path를 얻을 수 있는 제어 신호를 알고있다는 점에서 큰 이점을 갖는다. 특히 수많은 경우의 수를 같은 path에서 특정 condition을 기준으로 해서 효과적인 subset을 뽑는 연구가 진행되었다. 대표적인 조건이 path diversity와 survivability이다. 

이렇듯 기존의 연구는 어떤 식으로든 구해진 path 중에서 제어에 효과적인 subset을 뽑는데 집중해왔지만 해당 path를 어떻게 뽑는지에 대해서는 많은 연구가 진행되지 않았다. 많은 경우에 interval-wise 등속, 혹은 등 가속 운동을 하는 경로를 뽑았다. 하지만 이러한 path set은 장애물의 위치, 혹은 goal의 위치를 전혀 고려하지 않기 때문에 실제 로봇에 적용되었을 경우 여러 문제점이 생길 수 있다. In fact, survivability가 말하는 것이 바로 주어진 path subset이 실제 상황에 적용되었을 때, 얼마나 효과적인지 (장애물에 걸리지 않는지) 에 대한 연구이다. 한가지 더 큰 문제는 goal을 고려하지 않는다는 점이고, 기존의 방법에선 장애물에 걸리지 않는 path 중에서 goal까지 거리가 제일 짧은 path를 사용해서 제어를 한다. 이때 goal 까지 거리는 단순 유클리디안 거리로 하던가, configuration을 고려한 graph-based distance로 한다. 만약 지도가 복잡하다면 이 거리 계산에 오랜 시간이 걸릴 수도 있고, goal까지 reachability를 보장하지 않으므로 dead-lock에 걸릴 수도 있다. 

이렇게 하는 이유는 기존의 알고리즘은 하나 혹은 여러 개의 point를 따라가는 path를 쉽게 뽑을 수 없기 때문이다. In fact, 특정 waypoint를 지나서 goal position까지 가는 문제 자체만으로도 non-trivial problem이라고 할 수 있다. RRT와 같은 single-query 알고리즘은 오로지 이러한 문제만을 위한 알고리즘이다. 이 논문에선 시작, 목표, 그리고 여러 waypoint가 주어졌을 때, 이러한 wp들을 지나면서 시작점과 목표점을 연결하는 수많은 곡선을 빠른 시간에 찾을 수 있는 방법에 대해서 연구하였다. 그림1은 제안한 알고리즘을 이용해서 찾은 세 점을 지나는 1,000 개의 곡선이다. 1,000 개의 곡선을 뽑는데 50ms가 걸리지 않았다. 만약 GPU를 사용한다면 훨씬 더 빠르게 뽑을 수 있다. 

그림1 1,000 paths passing through three waypoints. 

---

Proposed Algorithms

 우리는 Bayesian sampling 방법을 통해서 path를 뽑고자한다. 그러기위해서 먼저 path가 무엇인지 정의를 할 필요가 있다.

정의1: path

 f: t->(x, y) where t \in (0, T)

정의1과 같이 우리는 path를 시간과 공간 (이차원에 한정되지 않음)을 연결해주는 함수로 생각을 하겠다. Random sampling을 위해서는 먼저 우리는 path를 random variable로 보아야 한다. 하지만 시간은 continuous variable이기 때문에 path의 cardinality는 무한대라고 볼 수 있다. 이러한 무한 차원의 벡터, 혹은 함수, 에 대한 분포는 random process를 통해서 설정할 수 있다. Particularly, 우리는 Gaussian process prior를 path에 대한 prior로 설정을 하였다. Random process에서 하나의 sample, 함수, 를 뽑는 것을 realization이라고 하는데 만약 smooth한 kerenl function으로 정의된 Gaussian process에서 realize된 함수는 differentialble하다는 장점이 있다. 이러한 성질은 특히 non-holonomic constraint를 갖는 mobile robot에 중요하다. 

성질2: 만약 가우시안 프로세스의 커널이 smooth하다면, 여기서 뽑은 함수 역시 smooth하다. 

 이렇게 정의된 가우시안 프로세스를 통해서 우리는 임의의 path를 뽑을 수 있지만, 특정 점들을 지나는 곡선을 뽑을 수는 없다. 확률에서 특정 observation이 있을 때, 이에 condition된 확률을 posterior 분포라고 한다. 이와 비슷하게 주어진 waypoint들을 obsevation이라고 할 때, GP prior를 갖는 path의 conditional 분포를 Gaussian posterior distribution이라하고, 다음과 같이 유도된다. 

이 역시 Gaussain process이므로, mean function과 variance function으로 fully specified한다. 이렇듯 이 mean과 variance function을 이용해서 특정 점들을 지나는 곡선을 sampling을 할 준비가 다 되었다. GP path의 다른 큰 장점은 이것이 discretization에 free하다는 점이다. 즉 sampling을 할 때, 미리 정한 시간의 sequence의 간격이 전체 곡선의 성질에 영향을 주지 않는다는 점이다. 이는 RRT와 같은 incremental하게 path를 만들어가는 방법과 대비되는데, 이러한 알고리즘의 경우 samping period에 민감하다. 

또한 GP에서 각 waypoint의 expected measurement variance를 조정함으로써 resulting sampled path들이 각 waypoint에 얼마나 가깝게 지나갈지를 정할 수 있다. 구체적으로는 sig2w가 클 경우엔 해당 점을 loose하게 pass한다. 이러한 성질은 우리가 만약 특정 점에 정확히 가까이 가기보다 주변에 가는 path를 뽑기원할 경우에 유용하다. 주어진 점이 있을 때 이를 잇는 선을 찾는 알고리즘으론 Spline fitting이 있다. 하지만 이 방법은 곡선을 piecewise polynomial로 fitting을 하고, 우리의 방법은 non-parametric 방법으로 표현을 하므로, 더 많은 표현을 할 수 있다. 무엇보다 Spline fitting은 deterministic하게 곡선을 찾으므로 그 외 다른 곡선을 뽑을 방법이 없다. 

Gaussian process를 이용해서 곡선을 뽑기 때문에, 여러 다른 kernel을 이용해서 수많은 확장이 가능하다. 예를 들어 SE 커널을 사용한다면 infinite differentialble한 부드러운 곡선을 뽑는데 적합할 것이다. 만약 곡선에 주기성을 갖게하길 위한다면, periodic kernel을, 직진성을 갖게하기 위한다면 polynomial kernel을 갖게할 수 있다. 한가지 재밌는 사실은 polynomial kernel을 이용한 GP의 mean function은 spline fitting과 같은 효과를 갖는다. 특히 path가 있을 때 다음에 해당 path가 어디로 이어질지에 대해서도 대략적으로 구할 수 있다. 이 문제는 extrapolation이라고도 불리는데, kernel의 hyperparameter를 효과적으로 학습해야한다. 

 

---

Tracking

제안한 path sampling 방법은 tracking 문제에 적용될 수 있다. Tracking can be seen a s a special case of path planning problem of which the target is constantly changing. 이렇게 상황에 계속 변하기 때문에 기존 알고리즘들이 구현하기에 어려움이 있다. 특히 동적 환경에서 움직이는 사람을 따라가는 것은 어려운 문제이다. configuration space가 계속 변하기 때문에, 현재 상황에서 구한 path가 다음번에는 사용되기 어려울 수도 있기 때문에 매 control cycle마다 goal까지가는 새로운 path를 뽑고, 해당 path를 바탕으로 제어를 해야한다. 이 논문에선 partially observable egocentric view of a robot 만을 이용해서 주어진 target를 따라가는 알고리즘을 구현하였다. 

---

References

[1] Branicky, Michael S., Ross A. Knepper, and James J. Kuffner. "Path and trajectory diversity: Theory and algorithms.", ICRA 2008, IEEE 

[2] Knepper, Ross A., and Matthew T. Mason. "Path diversity is only part of the problem.", ICRA, 2009. IEEE

[3] Erickson, Lawrence H., and Steven M. LaValle. "Survivability: Measuring and ensuring path diversity." Robotics and Automation, 2009. ICRA'09. IEEE International Conference on. IEEE, 2009.

[4] Jung, Dongwon, and Panagiotis Tsiotras. "On-line path generation for unmanned aerial vehicles using b-spline path templates." Journal of Guidance, Control, and Dynamics, 2013