2021年7月25日 星期日

機器人的速度運動模型 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})\) 的機率即可。

沒有留言:

張貼留言