다음 waypoint 찾는데 local minima에 빠지는 문제

2m 간격으로 

 

현재 waypoint에서부터 현재 waypoint가 바라보는 방향으로 2m 간 점을 구하고, 그 점에서 가장 가까운 waypoint를 찾는 문제였는데, 계속해서 같은 점이 구해졌다.