Dubins path

이전 작업의 naive한 RRT에서 만들어진 path는 자동차가 가기에는 무리인 경우가 많았다. 이는 naive RRT에서의 가정 중 "차량이 어느 방향으로든 이동할 수 있다."이 있기 때문이었다. 그래서 그 가정을 없애기 위해 Dubins path를 활용해보고자 한다.

 

이 글은 위 논문("Classification of the Dubins set")과 '프로그래머스' 교육자료를 참고해서 작성하였다.

---

0. 개요

해당 주제를 공부하면서 몰랐던 그리고 필요했던 지식들을 가볍게 소개하는 파트이다.

 

- holonomic vs. Non-holonomic

로봇의 자유도와 로봇이 구동되는 평면의 자유도가 일치 하는 것.

제어가능한 자유도가 전체 자유도보다 크기가 같은 것....

(그림)

자동차는 Non-holonmic 이다.

 

- motion primitive

로봇이 갈 수 있는 경로를 미리 계산(offline)하고, 미리 계산된 경로를 이용해 planning(online)

차가 갈 수 있는 feasible 한 경로를 구할 수 있고, 다양한 구속조건(constraints)을 미리 고려할 수 있는 장점이 있다.

하지만 모든 space를 다 고려할 수는 없어 불완전성의 가능성 존재.

 

[참고 youtube 영상]

https://youtu.be/58gnA5ovUIQ

https://youtu.be/ZvUedi5mzN8

 

- 유클리디안 거리

두 점 사이에서 가장 가까운 거리

np.sqrt(dx**2 + dy**2)

(그림추가)

 

1. Dubins path

- dubins path란

곡률이 제한된 상황에서, 시작지점에서 목표지점까지 도달하는 가장 짧고 완만한 경로를 찾는 문제가 있다. 위 문제에서 가장 기초적으로 고려할 수 있는 것이 "Dubins path"이다. (non-holonomic motion planning에서 중심 역할을 하는 문제)

 

- 제시된 vehicle model

x' = cos(θ)

y' = sin(θ)

θ' = u

state : x, y, θ

input(u) : ρ(곡률, curvature)

(') = d/ds // 시간이 아닌 거리에 대한 미분, trajectory가 아닌 path를 다룬다.

 

- words

dubins path는 Left turn(L), Right turn(R), Straight(S)이 3가지의  motion primitive를 고려했다.

이 3개의 모션 요소를 조합해 1개의 상황 만들었고 이를 dubins path에서 word라고 표현한다.

dubins path에는 총 6가지의 조합이 존재한다. (six words : RSL, LSR, RSR, LSL, RLR, LRL)

 

Ref : Shortest path optimization of haul road design in underground mines using an evolutionary algorithm - Scientific Figure on ResearchGate.

예를들어 RSL의 경우 Right turn -> Straight -> Left turn을 하는 하나의 상황(word).

dubins path는 위 6가지의 상황 중에서 가장 짧은 경로(e.g. 위 figure에서 LSR)를 선택하는 것이다.

 

- Nomalized parameter

[논문]

[정리]

(inital point : Pi = Ps, final point : Pf = Pg)

논문에서는 Pi(시작 위치)와 Pf(도착 위치)를 x축 상의 같은 점(θ=0)으로 두어 설명하고 있고, 위 그림은 임의의 position에서(θ≠0) Nomalized parameter를 설명한다.

- Three elementary motions

인덱스 v에서 모션 요소인 Left turn(L), Right turn(R), Straight(S)에 대한 수식

인덱스 v는 t -> p -> q 순으로 진행하고 t, p, q각 모션의 인덱스이자 그 모션의 길이 자체가 된다. (자세한건 아래 예시참고)

=> 즉, path의 길이 L = t + p + q

 

- example : LSL

 

inital state : (0, 0, α), final state : (d, 0, β)

우리의 목적은 inital -> final 지점으로 가는 dubins path를 만드는 것.

 

우리는 words 중 LSL를 선택했다. 즉, inital state(0, 0, α)에서 LSL을 하면 final state(d, 0, β)가 된다는 것을 의미한다.

 

좀 더 자세하게 예를들면, 첫 번째는 Left turn(L)으로 논문에서 나온 Left turn 수식에 inital state(0, 0, α)를 대입한다. 그러면 2번째 처럼 새로운 state값이 나오게 되는데 이는 Left turn을 한번 했을 때의 state를 구한 것이다.

 

이 state를 처음에 inital state를 Left turn 수식에 대입한 것처럼 Straight 수식에 대입하고 그 결과를 마지막 Left turn 수식에 다시 대입하면 최종적인 state가 나오게 되는 것이다.

 

그런데 우리는 최종적인 state인 final state(d, 0, β)를 알고 있다. 다시 말하면,

 

(위 방법으로 구한 state) = (final state)

 

이렇게 3개의 연립방정식이 만들어진다. 여기서 우리가 알고 싶은 값은 t, p, q 이다.

 

결국, dubins path는 각 모션의 거리인 t, p, q를 구하는 문제인 것이다.

 

+a : 여기서 {mod 2π}는 해당 값이 0~2π 사이의 값을 가진다는 의미.

 

- Dubins path 정리

  1) d, α, β 계산

  2) 모든 dubins path의 t, p, q 계산 후 각 path 마다 전체 길이 도출 

  3) 그 중 path의 전체 길이가 가장 짧은 path 선택  <-- 이 path가 dubins path가 되는 것.

 

2. python 구현

- main 부분

def plan(self, state1, state2, kappa):
    '''
    input : 시작 & 도착 state, 자동차가 가능한 minimum radius(최소 회전반경)
    output : dubins path, 이를 위한 control 값, cartesian 좌표계에서의 path
    '''
    dx = state2[0] - state1[0]
    dy = state2[1] - state1[1]
    th = np.arctan2(dy, dx)

    d = np.hypot(dx, dy) * kappa
    alpha = twopify(state1[2] - th)
    beta = twopify(state2[2] - th)

    dubins_path = self.get_best_dubins_path(d, alpha, beta) # dubins path 중 가장 짧은 path 선택됨
    controls = self.dubins_path_to_controls(dubins_path, kappa) # kappa를 이용해 control param 구함
    cartesian_path = self.controls_to_cartesian_path(controls, state1) # cartesian 좌표의 path 구함

    return cartesian_path, controls, dubins_path

 

- dubins path 계산하는 부분

def get_best_dubins_path(self, d, alpha, beta):
    '''
    input : nomalized parameter
    output : minimum path
    '''
    # six words
    dubins_functions = [
        self.dubinsLSL, self.dubinsRSR, self.dubinsLSR, 
        self.dubinsRSL, self.dubinsRLR, self.dubinsLRL
    ]

    # minimum path 구하기
    min_length = 1e10
    path = None
    for dubins_function in dubins_functions:
        tmp_path = dubins_function(d, alpha, beta)
        if tmp_path is not None:
            if (tmp_path.length() < min_length):
                min_length = tmp_path.length()
                path = tmp_path

    return path

 

- six words 부분 (ex. LSL) 

def dubinsLSL(self, d, alpha, beta):
    ca, sa = np.cos(alpha), np.sin(alpha)
    cb, sb = np.cos(beta), np.sin(beta)
    tmp = 2 + d*d - 2*(ca*cb + sa*sb - d*(sa - sb))
    if (tmp >= self.constant["dubins_zero"]): # arctan 정의역 만족하는
        theta = np.arctan2(cb-ca, d+sa-sb)
        t = twopify(-alpha + theta) # mod 2pi
        p = np.sqrt(np.amax([tmp, 0]))
        q = twopify(beta - theta)

        return DubinsPath(t, p, q, ["L", "S", "L"])

    else:
        return None

 

전체 코드는 추후에 git 업로드 ....

 

3. 결과확인

RSR의 path가 최소이므로 dubins path로 RSR이 반환되어 출력된 결과이다. 

 

가능한 모든 path의 길이를 뽑아 비교해 보아도 역시 RSR의 길이가 가장 짧은 것을 알 수 있다. 확인한 결과 RSL은 path가 만들어지지 못했다. 이는 정의역의 범위를 고려한 if 문에서 걸러진 것으로 보이고 루트 안의 값인 tmp가 음수가 나온 것을 확인하였다.

 

다른 시드를 살펴보니 이번엔 RSL의 길이가 가장 짧은 것을 볼 수 있다.

 

마지막으로 다양한 상황에서 dubins path를 구한 결과들이다. 처음 이 path를 보았을 땐 왜 이렇게 불필요하게 이동하지라고 생각했지만, 해당 state에서 heading각을 맞춰야 한다는 것과 최소 반경이 있다는 사실이 그 생각을 빠르게 바꾸었다.

 

4. summary

dubins path는 non-holonmic motion planning 문제 중 하나인 vehicle이 갈 수 있는 fesible한 경로를 생성한다.

dubins path는 3가지 motion primitve를 고려했고 이 모션 요소를 조합해 6가지의 상황(word)을 만들었다.

dubins path는 각 모션에서의 길이 t, p, q를 구하는 것이 point다.

dubins path에서는 가능한 정의역을 제한하는 것이 필요하다.

 

다음에는 offline에서 만든 dubins path를 가지고 non-holonmic motion planning을 해보자.