一個剛性移動機器人的構型通常用6個變量來描述：他的三維直角坐標系，以及相對外部坐標系的三個歐拉角（RPY 橫滾 ，俯仰，偏航），所以那么在平面環境中一般用三個變量既可以描述，稱之為位姿。
所以一般而平面上機器人的位姿信息也就是機器人的二維平面坐標（x,y）及其方位角θ，用這個向量來表示：

???xyθ???
沒有方向的位姿就叫做位置（Location）.那我們知道概率運動學中的條件密度是： p(xt|ut,xt?1)
我們都是知道的 xt和 xt?1都是機器人的位姿， ut是運動控制，所以這個模型描述了對 xt?1執行運動控制 ut后，機器人索取得的運動學狀態的后驗分布。

速度運動模型是通過兩個速度：平移速度和旋轉速度來控制機器人
所以控制ut=(vtwt)
規定逆時針旋轉角速度為正，向前運動線速度為正。以下直接給出
motion_model_velocity的偽代碼

其中第二行最難理解，雖然我是自我理解了 但是不知道對還是不對，所以還是不寫了，上偽代碼中的具體含義：初始位姿xt?1=(x?y?θ)T 控制ut=(v?w)T 和假想的后繼位姿xt=(x′?y′?θ′)T作為輸入，控制以Δt執行，參數α1?α6是機器人的運動誤差參數。

對于一些參數的設置：
Motion Model Parameters (all standard deviations of a gaussian noise model) 運動模型的參數 高斯噪聲模型的所有標準偏差
- “~/srr” [double] linear noise component (x and y) 線速度的噪聲分量
- “~/stt” [double] angular noise component (theta) 角速度的噪聲分量
- “~/srt” [double] linear -> angular noise component 線速度與角速度之間的噪聲分量
- “~/str” [double] angular -> linear noise component 角速度與線速度的噪聲分量

      <param name="srr" value="0.1"/><param name="srt" value="0.2"/><param name="str" value="0.1"/><param name="stt" value="0.2"/>

誤差的標準方差與給定速度成正比，指定的誤差參數，建立了機器人的準確性模型，一個機器人越不精確，這些參數就會越大。
所以以上是關于基于速度信息計算p(xt|ut,xt?1)的算法。

接下來就是要從運動模型中采樣，粒子濾波并不計任意xt,ut，xt?1的后驗。采樣時，給定ut和xt?1是為了根據從運動模型p(xt|ut,xt?1)產生一個隨機的xt,接下來就是根據運動模型的采樣算法：

此算法的輸入是xt?1和ut根據之前的分布p(xt|ut,xt?1)產生一個隨機位姿xt，第2-4行是有運動學模型的誤差參數來產生的新的（第5-7行）樣本位姿，具體的代碼如下實現：

//這里有兩個drawFromMotion函數可以用于函數的重載
//參數是   輸出位姿  線速度  角速度   這其中是要根據《速度運動模型》寫函數   具體的理論中文版《概率機器人》P90
OrientedPoint 
MotionModel::drawFromMotion (const OrientedPoint& p, double linearMove, double angularMove) const{OrientedPoint n(p);//用于存儲位姿double lm=linearMove  + fabs( linearMove ) * sampleGaussian( srr ) + fabs( angularMove ) * sampleGaussian( str ); //加了噪聲的線速度對應第2行double am=angularMove + fabs( linearMove ) * sampleGaussian( srt ) + fabs( angularMove ) * sampleGaussian( stt ); //加了噪聲的角速度n.x+=lm*cos(n.theta+.5*am);n.y+=lm*sin(n.theta+.5*am);n.theta+=am;n.theta=atan2(sin(n.theta), cos(n.theta));return n;
}

以上是主要的根據運動模型的一個代碼，很明顯里面有一個函數sampleGaussian（）是十分重要的。sampleGaussian（b2）是產生一個方差為b2的以0為中心的分布的一個隨機樣本。可以通過一下的算法實現

這個算法是從均值為0 方差為b2的近似正態分布和三角分布中采樣的算法，其中函數rand(x,y)是在[?b,b]中均勻分布的一個偽隨機數的產生器。
那么程序中出現 fabs( linearMove ) * sampleGaussian( srr ) 意思就是產生一個峰值為fabs( linearMove ) 方差為srr的隨機樣本。這也是為什么這句話中 n.x+=lm*cos(n.theta+.5*am); 為什么乘以0.5的原因。
。
下圖給出了采樣程序的結果

對比知道第一副是具有中等誤差的采樣結果，第二幅圖是具有較小的角度誤差和較大的平移誤差的采樣結果，第三圖是具有較大的角度誤差，較小的平移誤差的結果。

*這是我個人的理解，可能有一些偏差，或者錯誤，有錯誤還請指正，當然不喜勿噴

總結

以上是生活随笔為你收集整理的《概率机器人》速度运动模型gmapping中代码解析的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。