機器人的里程表運動模型 Odometry Motion Model

本文主要內容為 Probabilistic Robotics 書中的 5.4：Odometry Motion Model。首先書上提到了 Odometry motion model 雖然有誤差，但實際經驗中仍然比前文的 Velocity Motion Model 精確。Odometry Motion Model 的控制資訊由機器人內部的里程表測量，提供的是機器人內部座標系統裡的位置： \[ \bar{x}_{t-1} = [\bar{x}, \bar{y}, \bar{\theta}]^T \\ \bar{x}_{t} = [\bar{x'}, \bar{y'}, \bar{\theta'}]^T \\ u_t = \begin{bmatrix} \bar{x}_{t-1} \\ \bar{x}_{t} \end{bmatrix} \] 為了求出相對的距離，\(u_t\) 被分解成旋轉 \(\delta_{rot1}\)、平移 \(\delta_{trans}\)、以及第二次旋轉 \(\delta_{rot2}\)，如下圖：

我們可以從 \(u_t\) 推出這三個參數的關係式： \[ \delta_{rot1} = atan2(\bar{y'}-\bar{y}, \bar{x'}-\bar{x})-\bar{\theta} \\ \delta_{trans} = \sqrt{(\bar{x'}-\bar{x})^2+(\bar{y'}-\bar{y})^2} \\ \delta_{rot2} = \bar{\theta'} - \bar{\theta} - \delta_{rot1} \] 而在真實世界中我們必須考慮噪音產生的誤差模型： \[ \hat{\delta}_{rot1}=\delta_{rot1}-\varepsilon_{\alpha_1\delta_{rot1}^2+\alpha_2\delta_{trans}^2} \\ \hat{\delta}_{trans} = \delta_{trans}-\varepsilon_{\alpha_3\delta_{trans}^2+ \alpha_4\delta_{rot1}^2 + \alpha_4\delta_{rot2}^2 }\\ \hat{\delta}_{rot2} =\delta_{rot2}-\varepsilon_{\alpha_1\delta_{rot2}^2+\alpha_2\delta_{trans}^2} \] 有了這個誤差模型以後，我們就可以推出位置狀態 \(x_{t-1}\) 與 \(x_t\) 的關係式了： \[ \begin{bmatrix} x'\\ y'\\ \theta' \end{bmatrix} = \begin{bmatrix} x\\ y\\ \theta \end{bmatrix} + \begin{bmatrix} \hat{\delta}_{trans}\ cos(\theta+\hat{\delta}_{rot1})\\ \hat{\delta}_{trans}\ sin(\theta+\hat{\delta}_{rot1})\\ \hat{\delta}_{rot1}+\hat{\delta}_{rot2} \end{bmatrix} \] 也就是說我們只要能產生噪音模型的樣本，就能從上個時間的位置狀態 \(x_{t-1}\) 及此時的運動資訊 \(u_t\) 來產生此時位置狀態 \(x_t\)的樣本。另外，我們也能算出運動的機率模型 \(p(x_t|u_t,x_{t-1})\)了。