5.3. Velocity Motion Model

이 글은 Sebastian thrun의 Probabilistic Robotics를 보고 내용을 정리한 글이며, 나름 쉽게 표현하기 위해서 의역을 한 부분이 있습니다.

 

 

운동학 구성(Kinematic Configuration)

운동학(Kinematics)은 로봇의 구성에서의 컨트롤 액션의 효과를 묘사하는 미적분입니다.  강체(rigid) 모바일 로봇의 구성(configuration)은 일반적으로 6개의 변수, 외부 프레임 좌표계에 대한 3차원 직교좌표계 및 3개의 오일러 각(roll, pitch, yaw)로 나타냅니다.

 

평면에서의 로봇의 자세(pose)는 그림 5.1에 나타내었습니다. 

그림 5.1. 전역(global)좌표계에 대해 나타낸 로봇의 자세

이는 외부 프레임 좌표계에 대해 각도 방향을 포함한 2차원 평면 좌표계로 구성됩니다. 평면 좌표는 $x$와 $y$로 나타내며(상태 변수 $x_t$와 헷갈리지 마세요), 각도는 $\theta$로 나타냅니다. 따라서 로봇의 pose는 다음과 같이 벡터 형태로 나타낼 수 있습니다.

 

로봇의 방향은 보통 bearing 또는 heading direction이라고 합니다. 그림 5.1에서, 만약 $\theta = 0$이면 로봇은 $x$축 방향을, $\theta = 0.5\pi$이면 $y$축 방향을 바라본다고 하겠습니다.

 

pose 값에서 방향값을 빼면 우리는 이 값을 위치(location)이라고 하며, 다음과 같이 $x-y$좌표계로 나타냅니다.

 

 

확률적 운동학(Probabilistic Kinematics)

확률적 kinematic model 또는 motion model은 모바일 로보틱스에서 상태 과도 모델의 역할을 합니다. 이 모델은 이전에 설명했던 익숙한 조건부 확률밀도로 다음과 같이 주어집니다.

 

여기서 $x_t$와 $x_{t-1}$은 로봇의 pose(단순히 $x$좌표값만을 의미하는게 아닙니다)이고, $u_t$는 운동 명령입니다. 이 모델은 로봇이 $x_{t-1}$에서의 운동 명령 $u_t$를 실행할 때 가정하는 kinematic state의 전체 사후 확률 분포를 묘사합니다. 때때로 $u_t$는 로봇의 오도메트리(odometry)로 주어지지만, 개념적인 이유에서 일단 $u_t$는 컨트롤이라고 하겠습니다.

 

그림 5.2는 평면 환경에서 움직이는 강체 모바일 로봇의 두 가지 kinematic 모델 예제를 보여줍니다. 두 경우 모두, 로봇의 초기 pose는 $x_{t-1}$입니다. 확률 분포 $p(x_t | u_t, x_{t-1})$은 어두운 영역으로 나타냈으며, 색이 더 어두울수록 더 높은 확률을 가집니다.

 

그림 5.2. 운동 모델(motion model) : 실선으로 나타낸 운동 명령을 실행했을 때의 로봇의 pose의 사후확률분포. 색이 더 어두울수록 더 높은 확률을 가진다. 이 그림은 2차원 평면에 사영되었으며, 원래의 확률 밀도는 로봇의 방향 θ를 고려한 3차원 데이터이다.

 

그림 5.2의 (a)에서, 로봇은 약간의 거리를 앞으로 움직이는 동안 그림에 나타냈듯이 회전 및 직선(translational error) 오차가 생깁니다. (b)는 더 복잡한 움직임을 하며, 이에 따라 더 넓게 퍼진 불확실성의 결과가 나왔습니다.

 

실제로, 오도메트리(odmetry)모델은 속도 모델보다 더 정확한 경향이 있는데, 대부분의 상업적 로봇은 로봇의 바퀴 회전을 측정하여 얻을 수 있는 정확도 수준으로 속도 명령을 실행하지 않기 때문입니다. 하지만, 오도메트리는 운동 명령이 실행되어야 계산 가능합니다. 따라서 이는 운동 계획(motion planning)에 사용될 수 없습니다. 장애물 회피같은 planning 알고리즘은 그 운동의 영향을 반드시 계산해야 합니다. 따라서, 오도메트리 모델은 일반적으로 추정에 적용되고, 속도 모델이 확률적 motion planning에 적용됩니다.

 

5.3. 속도 운동 모델(Velocity Motion Model)

velocity motion model은 우리가 로봇을 회전속도 및 선속도(translational velocity)를 통해 제어한다고 가정합니다. 시간 $t$에서의 선속도는 $v_t$라고 하고, 회전속도는 $w_t$라고 하겠습니다. 즉,

 

여기서 $v_t$는 로봇의 전방을 향해 움직일 때 +값의 속도를 가진다고 하고, $w_t$는 반시계방향으로 회전할 때 +값의 회전속도를 가진다고 하겠습니다.

 

1. Closed Form Calculation

table 5.1은 확률 $p(x_t | u_t, x_{t-1})$을 계산하기위한 알고리즘을 나타냅니다.

 

table 5.1. 속도 정보에 기반한 사후확률 p(x_t u_t, x_{t−1})을 구하기 위한 알고리즘.

 

이 알고리즘은 초기 pose $x_{t-1} = (x\; y\; \theta)^T$와 컨트롤 $u_t = (v\; w)^T$를 입력으로 받고, 추측하는 pose를 $x_t = (x'\; y'\; \theta ')^T$로 나타냅니다. 출력값은 상태 $x_{t-1}$에서의 컨트롤 $u_t$를 실행한 후 $x_t$에서의 사후확률 $p(x_t | u_t, x_{t-1})$이며, 컨트롤은 고정된 duration $\Delta t$동안 수행됐다고 가정합니다. 파라미터 $\alpha _1$부터 $\alpha _6$은 robot-specific motion error 파라미터 입니다. 이 알고리즘은 먼저 오차가 없는 로봇의 컨트롤을 계산합니다. 이 파라미터들은 $\hat{v}, \hat{w}$로 주어집니다. 여기서 $\gamma$는 로봇이 목표지점에 도착한 후의 회전을 나타냅니다. 실제 상황에서 로봇이 목표지점으로 움직일 때, 로봇이 목표지점에 도착하는 목표 pose에서 방향$(\theta)$까지 정확하게 맞춰서 도착하지는 않고, 일단 해당 위치로 이동한 후에 제자리에서 회전하면서 pose를 맞추게 됩니다. $\gamma$는 이렇게 도착지점에 도착한 후의 회전을 말합니다.

 

여기서 함수 $\bf prob \it (x, b^2)$는 운동 오차를 모델링한 것입니다. 이것은 파라미터 $x$가 중심이 0이고 분산이 $b^2$인 확률분포를 계산합니다. 이를 적용한 정규분포 및 삼각분포는 table 5.2.에 나타냈습니다.

 

Table 5.2. 평균이 0이고 분산이 b^2일때 각각의 분포의 계산

 

그림 5.3.은 $x-y$ 평면으로 사영된 velocity motion model의 예시를 보여줍니다.

 

그림 5.3. 각각 다른 노이즈 파라미터 설정을 했을 때의 velocity motion model

 

그림 5.3.의 모든 경우에서 로봇은 같은 선속도와 회전속도로 운동하도록 설정하였습니다. 그림 5.3.의 (a)는 $\alpha _1$부터 $\alpha _6$까지의 오차 파라미터를 중간 크기로 설정한 경우입니다. (b)는 작은 회전 오차($\alpha _3, \alpha _4$)를 설정하였으나 큰 직선(translational error) 오차($\alpha _1, \alpha _2$)를 설정한 경우입니다. (c)는 회전 및 직선 오차를 모두 크게 설정한 경우의 결과입니다.

 

 

2. 샘플링 알고리즘(Sampling Algorithm)

파티클 필터는 임의의 $x_t, u_t, x_{t-1}$의 사후 확률을 계산하는 대신 motion model $p(x_t | u_t, x_{t-1})$으로부터 샘플링을 하기에 충분합니다. 조건분 확률 밀도로부터 샘플링을 하는것은 그 확률 밀도를 계산하는 것과는 다릅니다. 샘플링에서는, 우리는 $u_t$와 $x_{t-1}$이 주어지고, motion model $p(x_t | u_t, x_{t-1})$을 따라 그려진 랜덤한 $x_t$를 생성하려고 합니다. 확률 밀도를 계산할때는, 다른 평균을 통해 생성된 $x_t$ 또한 주어지며, 조건 $p(x_t | u_t, x_{t-1})$에서의 확률 $x_t$를 계산하려고 합니다.

 

table 5.3.에서의 sample_motion_model_velocity 알고리즘은 고정된 컨트롤 $u_t$와 pose $x_{t-1}$에 대한 $p(x_t | u_t, x_{t-1}$로부터 랜덤한 샘플을 생성합니다.

 

Table 5.3. pose x_{t-1}로부터 x_t를 샘플링하는 알고리즘

이 알고리즘은 $x_{t-1}$과 $u_t$를 입력으로 받고, 분포 $p(x_t | u_t, x_{t-1})$를 따르는 랜덤한 pose $x_t$를 생성합니다. line 2부터 4는 컨트롤 명령을 kinematic motion model에서 나온 오차 파라미터 노이즈를 추가하여 "교란(perturb)"합니다. 그리고 그 노이즈 값들은 line 5~7에서 새로운 pose를 생성하기 위해 사용됩니다. 따라서, 샘플링 과정은 가장 간단한 방법으로 그 예측에서의 컨트롤 노이즈를 포함하는 간단한 로봇 운동 모델을 구현합니다. 그림 5.4는 그 샘플링 루틴의 결과를 보여줍니다. 여기서 함수 $\bf sample \it (b^2)$는 평균이 0이고 분산 $b^2$으로부터 랜덤한 샘플을 생성합니다.

 

그림 5.4. 그림 5.3과 같은 파라미터 값으로 500개의 샘플링을 한 결과

 

그림 5.4.의 샘플은 sample_motion_model_velocity 알고리즘에 의해 생성되었습니다. 

 

많은 경우에서 주어진 확률 밀도 $x_t$를 계산하는 것 보다는 $x_t$를 샘플링 하는것이 더 쉽습니다. 왜냐하면 샘플들은 단지 물리적인 운동 모델의 forward simulation만을 요구하기 때문입니다. 오차 파라미터의 역추측(retro-guessing)에 해당하는 예측되는 pose의 확률을 계산하기 위해서는 물리적 운동 모델의 역(inverse)을 계산해야 합니다.