機器人的速度運動模型 Velocity Motion Model

本文主要內容為 Probabilistic Robotics 書中的 5.3：Velocity Motion Model。 

本節假設機器人在二維空間中移動，因此用 \(x, y, \theta\) 三個變數來描述機器人位置的狀態： \[ \mathbf{x}_{t} = \begin{bmatrix} x\\ y\\ \theta \end{bmatrix} \] 而機器人的速度模型用平移速度 \(v\) 與角速度 \(\omega\) 來表示： \[ \mathbf{u}_{t} = \begin{bmatrix} v\\ \omega \end{bmatrix} \] 我們想要算出來的是：

• 運動模型的機率，也就是 \(p(x_t | u_t, x_{t-1})\)
• 產生一個符合運動模型機率分布的隨機樣本

速度模型的數學推導

我們用下圖來幫助解釋：

• 當機器人的平移速度為 \(v\)，角速度為 \(\omega\) 時，機器人的移動軌跡會是一個半徑為 \(r\) 的圓弧，而 \(v = r \omega\)。
• 此圓弧的圓心是左下角的點 \(<x_c, y_c>\)，我們可以推算出此圓心的位置： \[ \begin{bmatrix} x_c\\ y_c \end{bmatrix} = \begin{bmatrix} x - r\ sin\theta\\ y + r\ cos\theta \end{bmatrix} \] 
• 在短暫的時間 \(\Delta t\) 以後的位置狀態假設為 \([x', y', \theta']\)： \[ \begin{bmatrix} x'\\ y'\\ \theta' \end{bmatrix} = \begin{bmatrix} x_c + r\ sin(\theta + \omega \Delta t)\\ y_c - r\ cos(\theta + \omega \Delta t)\\ \theta + \omega \Delta t \end{bmatrix} \\= \begin{bmatrix} x\\ y\\ \theta \end{bmatrix} + \begin{bmatrix} -r\ sin\theta + r\ sin(\theta + \omega \Delta t)\\ r\ cos\theta -r\ cos(\theta + \omega \Delta t)\\ \omega \Delta t \end{bmatrix} \] 
• 但是在真實情況下，機器人的平移速度與角速度會受到噪音干擾，而噪音的模型為： \[ \begin{bmatrix} \hat{v}\\ \hat{\omega} \end{bmatrix} = \begin{bmatrix} v\\ \omega \end{bmatrix} + \begin{bmatrix} \varepsilon_{\alpha_1v^2+\alpha_2\omega^2} \\ \varepsilon_{\alpha_3v^2+\alpha_4\omega^2} \end{bmatrix} \] 另外為了能更加泛化此噪音模型，我們再加上此式子：\[ \theta' = \theta + \hat{\omega}\Delta t + \hat{\gamma}\Delta t \]
• 我們晚點再介紹噪音模型的細節。當把速度的噪音考慮進去後，再將 \(r\) 代換成 \(\frac{\hat{v}}{\hat{\omega}}\)，下一個時間點的位置狀態 \(x', y', \theta'\) 變成： \[ \begin{bmatrix} x'\\ y'\\ \theta' \end{bmatrix} = \begin{bmatrix} x\\ y\\ \theta \end{bmatrix} + \begin{bmatrix} -\frac{\hat{v}}{\hat{\omega}}sin\theta+\frac{\hat{v}}{\hat{\omega}}sin(\theta+\hat{\omega}\Delta t)\\ \frac{\hat{v}}{\hat{\omega}}cos\theta-\frac{\hat{v}}{\hat{\omega}}cos(\theta+\hat{\omega}\Delta t)\\ \hat{\omega}\Delta t + \hat{\gamma}\Delta t \end{bmatrix} \] 

 噪音模型

我們用的噪音模型是一個平均為\(0\)，變異數為 \(b^2\) 的機率分布。我們通常用常態分布或是三角分布當作噪音模型，而用以下的演算法可以產生符合這兩種機率分布的樣本：

• 常態分布：\(\frac{1}{2}\sum_{i=1}^{12}rand(-b, b)\)
• 三角分布：\(\frac{ \sqrt{6} }{2}[rand(-b,b)+rand(-b,b)]\)
  

\(rand(-b,b)\) 是一個均勻分布的隨機亂數生成器，能夠產生 \(-b\) 至 \(b\) 範圍中的隨機數字。

產生符合運動模型機率分布的隨機樣本的方法

此演算法的目的是在給定前一個時刻的位置狀態 \(x_{t-1}\) 及此時刻的控制資訊 \(u_t\) 後，產生一個符合現在位置狀態 \(x_t\) 機率分布的樣本。

1. 產生經過噪音擾動的控制資訊： \[ \left\{\begin{matrix} \hat{v} = \mathbf{sample}(\alpha_1v^2 + \alpha_2 \omega^2)\\ \hat{\omega}= \mathbf{sample}(\alpha_3v^2 + \alpha_4 \omega^2)\\ \hat{\gamma}= \mathbf{sample}(\alpha_5v^2 + \alpha_6 \omega^2) \end{matrix}\right. \] 
2. 將結果代入上面計算 \([x', y', \theta']\) 的式子

這樣就能產生一個樣本了。

算出運動模型機率的方法

最後一個問題是要算出對於給定前一個時刻的位置狀態 \(x_{t-1}\) 及此時刻的控制資訊 \(u_t\) 後，在此時刻位置狀態為 \(x_t = [x', y', \theta']\) 的機率。書中的 Table 5.1 寫下了此演算法，但由於推導過程有一點複雜，本文就先暫時跳過，只要記得此演算法的精神為算出 \(p(x_t | u_t, x_{t-1})\) 的機率即可。