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

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

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

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

對于一些參數(shù)的設(shè)置：
Motion Model Parameters (all standard deviations of a gaussian noise model) 運(yùn)動模型的參數(shù) 高斯噪聲模型的所有標(biāo)準(zhǔn)偏差
- “~/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"/>

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

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

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

//這里有兩個drawFromMotion函數(shù)可以用于函數(shù)的重載
//參數(shù)是   輸出位姿  線速度  角速度   這其中是要根據(jù)《速度運(yùn)動模型》寫函數(shù)   具體的理論中文版《概率機(jī)器人》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 ); //加了噪聲的線速度對應(yīng)第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;
}

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

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

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

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

總結(jié)

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

如果覺得生活随笔網(wǎng)站內(nèi)容還不錯，歡迎將生活随笔推薦給好友。