5.4. Odometry Motion model

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

 

 

 

 

이전 포스팅에서 설명한 velocity motion model은 로봇의 pose에 대한 사후 확률을 계산하기 위해 로봇의 속도를 사용했습니다. 그 대안으로, 시간에 대한 로봇의 움직임을 계산하기 위한 기저(basis)로써 오도메트리(odometry) 측정을 사용할 수도 있습니다. 오도메트리는 보통 휠 인코더 정보를 통합(integrate)하여 얻게됩니다. 대부부의 상업적인 로봇들은 그런 통합된 pose 추정을 주기적인 시간마다 제공합니다.

 

오도메트리 모델도 여전히 에러가 있긴 하지만, 실질적인 경험상 일반적으로 속도 모델 보다는 더 정확합니다. 두 경우 모두 노이즈와 미끄러짐(slippage) 때문에 곤란하긴 하지만, 속도 모델은 실제 motion controller와 그것의 수학적 모델 사이의 불일치까지 고려해야합니다. 하지만, 오도메트리는 로봇이 움직인 후 그 과정을 재확인하는 경우에만 사용할 수 있습니다. 이 오도메트리를 통한 pose는 나중에 소개할 로봇 위치 추정이나 맵핑(mapping)알고리즘같은 필터 알고리즘에는 문제가 없지만, motion planning이나 control에는 필요가 없습니다.

 

 

1. Closed Form Calculation

엄밀히 말해서, 오도메트리 정보는 센서 측정이지, 컨트롤이 아닙니다. 오도메트리를 측정값으로써 모델링하기 위해, 베이즈 필터 결과는 실제 속도값을 상태 변수(state variable)로써 포함할 것입니다(이 것은 상태 공간의 크기를 증가시킵니다). 상태 공간(state space)를 작게 유지하기 위해서는 일반적으로 오도메트리 데이터를 컨트롤 신호처럼 생각합니다. 이 포스팅에서는 오도메트리 측정을 컨트롤처럼 다룰것입니다.

 

이제 우리의 컨트롤 정보에 대한 포맷(format)를 정의하겠습니다. 시간 $t$에서, 적절한 로봇의 pose는 랜덤 변수 $x_t$로 모델링 됩니다. 로봇의 오도메트리는 이 pose를 추정하지만, 노이즈와 미끄러짐 때문에 로봇 내부의 오도메트리와 실제 physical world 좌표계 사이의 어떤 고정된 좌표 변환은 존재하지 않습니다. (사실, 이 변환을 알면 로봇의 위치 추정 문제를 해결할 수 있습니다.)

 

오도메트리 모델은 로봇 내부의 오도메트리에 의해 측정된 상대적 운동 정보(relative motion information)를 사용합니다. 자세히 말하자면, 시간 구간 $(t-1,t]$에서 로봇은 pose $x_{t-1}$에서 $x_t$로 이동합니다. 오도메트리는 그 후에 ${\bar{x}}_{t-1}=(\bar{x}\bar{y}\bar{\theta }{)}^T$에서 ${\bar{x}}_t=({\bar{x}}^{\prime }{\bar{y}}^{\prime }{\bar{\theta }}^{\prime }{)}^T$ 에 관련된 이동을 나타냅니다. 여기서 문자 위의 bar는 로봇 내부 좌표에 내장 된 오도메트리 측정으로 global world coordinates와의 관계는 알 수 없습니다.  여기서 이 정보를 상태 추정에서 사용하기 위해 중요한 것은 ${\bar{x}}_{t-1}$과 ${\bar{x}}_t$의 상대적인 차이이고, 그 "차이"는 true pose $x_{t-1}$과 $x_t$의 차이를 위한 좋은 estimator 입니다. 따라서, 이 쌍은 다음과 같이 주어집니다.

 

 

상대적인 오도메트리를 추출(extract)하기위해서 $u_t$는 회전, 직선 운동, 또다른 회전 이렇게 세 단계로 변환됩니다. 그림 5.7은 이 과정을 보여줍니다. 초기의 회전은 ${\delta }_{rot1}$, 직선운동은 ${\delta }_{trans}$, 두 번째 회전은 ${\delta }_{rot2}$입니다.

 

그림 5.7. 오도메트리 모델

 

각각의 위치 쌍 $(\bar{s}{\bar{s}}^{\prime })$은 유일한 파라미터 벡터 $({\delta }_{rot1}{\delta }_{trans}{\delta }_{rot2}{)}^T$를 가지며, 이 파라미터들은 $\bar{s}$와 ${\bar{s}}^{\prime }$사이의 상대적인 움직임을 재구성하기에 충분합니다. 따라서, ${\delta }_{rot1},{\delta }_{trans},{\delta }_{rot2}$은 오도메트리에 의해 인코딩된 상대적 운동의 충분한 통계를 형성합니다.

 

확률적 운동은 이 세 파라미터들이 독립적인 노이즈에 영향을 받는다고 가정합니다. 수학적으로 자세하게 들어가기 이전에, 이 확률 밀도를 계산하기 위한 기본적인 알고리즘을 보겠습니다. table 5.5는 오도메트리로부터 $p(x_t|u_t,x_{t-1})$를 계산하는 것을 보여줍니다.

 

table 5.5. 오도메트리 정보를 기반으로 사후확률을 구하는 알고리즘

이 알고리즘은 초기의 pose $x_{t-1}$, 오도메트리로부터 얻은 pose의 쌍 $u_t=({\bar{x}}_{t-1}{\bar{x}}_t{)}^T$, 예측 최종 pose $x_t$를 입력으로 받습니다. 출력은 수학적인 확률 $p(x_t|u_t,x_{t-1})$입니다.

 

table 5.5의 line 2부터 4는 relative motion parameter $({\delta }_{rot1}{\delta }_{trans}{\delta }_{rot2})$를 오도메트리를 읽는것으로부터 되찾습니다. 이전에, 이것들은 inverse motion model을 구현했습니다. 주어진 pose $x_{t-1}$와 $x_t$에 대응되는 motion parameter $({\hat{\delta }}_{rot1};{\hat{\delta }}_{trans}{\hat{\delta }}_{rot2}{)}^T$는 line 5부터 7을 통해 계산되었습니다. line 8부터 10은 각각의 motion parameter들의 오차 확률을 계산합니다. 함수 $\mathbf{p}\mathbf{r}\mathbf{o}\mathbf{b}\mathit{(}\mathit{a}\mathit{,}{\mathit{b}}^{\mathit{2}}\mathit{)}$는 평균이 0이고 분산이 $b^2$인 분포를 구현합니다. 여기서 반드시 확인해야 할 점은 각의 차이는 구간 $[-\pi ,\pi ]$에 있어야 한다는 것입니다. 결과값 ${\delta }_{rot2}-{\bar{\delta }}_{rot2}$는 그 구간에 맞게 잘라져야 하므로 이는 디버깅을 어렵게 만듭니다. 마지막으로 line11은 각각의 에러 확률 $p_1,p_2,p_3$를 곱하여 얻어진 조합된 에러 확률을 반환합니다. 이 마지막 단계는 각 다른 에러들이 독립이라고 가정합니다.

 

그림 5.8. 파라미터 설정에 따른 오도메트리 운동 모델

그림 5.8은 ${\alpha }_1$부터 ${\alpha }_4$의 파라미터들을 다르게 설정했을 때의 예시를 보여줍니다. (a)는 일반적인 예시이며, (b)와 (c)는 각각 직선 오차를 크게 한 경우, 회전 오차를 크게 한 경우입니다. 연속된 두 개의 측정에서 시간 차이가 작수록, 이 다른 운동 모델들은 더 비슷해집니다. 따라서, 만약 belief가 자주 업데이트된다면, 예를들어 일반적인 실내용 로봇이 1초에 10번씩 한다면, 이 모델들 간의 차이는 별로 크지 않습니다.

 

 

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

Table 5.6의 sample_motion_model_odometry는 샘플링 접근을 보여줍니다.

 

table 5.6. 오도메트리 정보에 기반한 사후확률의 확률에서 샘플링을 하는 알고리즘

 

위 알고리즘은 초기 pose $x_{t-1}$와 오도메트리 reading $u_t$를 입력으로 하며, 출력은 $p(x_t|u_t,x_{t-1})$를 따라 분포된 랜덤한 $x_t$입니다. 이 것은 주어진 $x_t$의 확률 계산하는 이전의 알고리즘과는 달리 랜덤하게 pose $x_t$를 추측합니다. 

 

그림 5.9. 오도메트리 모델로부터의 샘플링. 그림 5.8과는 같은 파라미터를 사용했으며, 500개의 샘플을 추출했다.

 

그림 5.10. non-sensing robot의 position belief 샘플링 근사

 

그림 5.10은 시간이 지남에 따른 로봇 운동 모델을 나타냈습니다. 이 데이터는 로봇의 오도메트리가 그림의 직선 경로를 따라간다고 가정했을 때, 파티클 필터 알고리즘(table 4.3.)의 motion update방정식을 이용하여 만들어졌습니다. 이 그림은 로봇이 움직임에 따라 불확실성이 어떻게 커지는지 보여줍니다.