運動模型--速度和里程計
在自動駕駛,輪子機器人的使用中,一般我們都是用平面世界中的運動模型來進行移動計算(預測)。在平面環境中運動的移動機器人, 它的運動學狀態,或者說是位姿pose,可以用三個變數來表示 。在平面中我們有兩個模型,一個是速度模型一個是里程計模型,里程計模型一般比速度模型更精確,因此大多數商業機器人並不使用速度模型。但是里程計模型具有一定的滯後, 因此不能用於運動規劃。蔽障的規劃演算法不得不預測運動的結果。因此,里程計模型通常用於估計,而速度模型用於概率的運動規劃。在之後的介紹中會詳細解釋這麼說的原因。
概率運動學模型,或者說是運動模型motion model在移動機器人的狀態轉移模型中扮演著重要的角色。 這個模型實際上就是一個我們熟悉的條件概率密度函數:
速度模型
代表的是機器人的方向角度,x,y是平面中所在位置。
代表發送給差分輪,轉機的速度命令,
。
這裡不做過多的推到,根據 ,可以推出
真實運動中實際速度與給定速度是有誤差的,並將其表示為均值為0,方差有限的隨機變數:
因此後的機器人的真實狀態,這裡用疊加雜訊後的運動
代替了給定速度
:
之後我們可以通過公式計算出在得知 的情況下,帶雜訊的真實的
在雜訊範圍內計算出真實的速度數據之後,就可以計算出prob(),這個函數就是計算真實速度(在指定方差範圍內)的後驗概率。裡面最大的的概率就是最有可能的最終 。如下例子:
這個演算法適用於卡爾曼濾波等。還有一種採樣演算法,適用於粒子濾波。公式如下:
原理都類似,只是該演算法由 依概率給出一個
與前圖類似,下圖為在運動模型中採樣的結果:
里程計
到目前為止,我們討論的運動模型都是根據機器人的速度計算位姿的後驗概率的模型。有時我們可能想用里程計的測量值作為機器人運動的計算基礎。 里程計通常通過對輪子編碼器數據的積分獲得,大多數商業機器人以一定的周期積分,進而估計位姿數據。這就是本章要討論的第二種運動模型——里程計運動模型odometry motion model。 這種模型用里程計的測量值來代替控制量。
實際經驗告訴我們儘管里程計也存在誤差,但它也比速度模型要準確很多。這兩種方法都要受到打滑、數據漂移的影響,此外速度模型還受限於實際運動控制器與數學模型之間的差異。 但是里程計具有一定的滯後性,只有在機器人運動之後才能夠獲得。這對於濾波器演算法而言不是什麼問題,但是對於準確的運動規劃和控制就不合適了。
我們先來看看里程計的演算法,再來解釋里程計適合估算的原因。
根據上圖我們可以看出里程計是從 到
通過旋轉--平移--旋轉
里程計公式如下:
同理,根據如下公式計算出 ,只是現在的值是trans和rot1,rot2.
同樣有採樣的演算法:
最終的目的和速度模型類似。
在路徑規劃的時候,對於預測是很重要的,我們需要在不知道下一步具體是怎麼走的情況下,規劃設定出下一步的動作行為,但是如何評估這個預測的動作的準確度是在哪裡呢?
可以由速度Ut來估算,odom不行,因為一定要具體的移動之後,encoder出來的數據進行估算。這樣在實際運動過程中,就delay了,無法及時獲取到最可靠,概率最高的路徑。而Ut則一直都在。
推薦閱讀: