里程計運動模型（odometery motion model）用距離測量代替控制。實際經(jīng)驗表明雖然里程計雖然仍存在誤差，但通常比速度運動模型更加的精確。相比于速度運動模型運動信息ut由

(xˉt?1xˉt)
為了提取相對的距離， ut被轉(zhuǎn)變?yōu)槿齻€步驟的序列：旋轉(zhuǎn)，平移，另一個旋轉(zhuǎn)，

上圖就是里程計的測距模型，同樣這些旋轉(zhuǎn)和平移都是有噪聲的。
首先由里程計算 p(xt|ut,xt?1)的算法，算法的輸入是機器人的初始位姿 xt?1，從機器人里程計獲得的一對位姿 ut=(xˉt?1,xˉt),以及一個假定的最終姿態(tài) xt，輸出的數(shù)值概率是 p(xt|ut,xt?1)

算法的2-4行是從里程計的讀數(shù)獲取相對運動參數(shù)(δrot1?δtrans?δrot2)T
第5-7行是相同的，就是計算位姿為xt?1 到x?t的相對運動參數(shù)，
第8-10行是計算誤差概率，
第11行返回各自的誤差概率P1 ，P2 ,P3相乘得到的組合誤差概率，假定不同誤差源之間是相互獨立的，變量α1?α4是指定機器人運動噪聲的機器人的特定參數(shù)。
基于里程計運動模型的采樣算法

數(shù)學推導：根據(jù)上上面的圖應(yīng)該不難推導出算法中的(δrot1?δtrans?δrot2)T，為了建立運動誤差模型，假設(shè)旋轉(zhuǎn)和平移的“真”值是測量值減去均值為0 方差為b2的獨立噪聲?b2得到也就是上述算法中的5-7行，為了計算旋轉(zhuǎn)和平移的真值引入了誤差參數(shù)α1?α4，所以實際位置xt，從xt?1經(jīng)過初始旋轉(zhuǎn)δ^rot1跟隨平移距離δ^trans和另一個旋轉(zhuǎn)δ^rot2得到

這里的時刻t的位姿用xt?1=(x?y?θ)T
那么根據(jù)給定的運動模型采樣算法，，對于不同的誤差參數(shù)也會有不同的概率分布：

第一個模型的采樣參數(shù)是中等的，可以說是正常的，第二個和第三個扥別是比較大的平移和旋轉(zhuǎn)的誤差所造成的。

Gmapping中實現(xiàn)里程計運動模型的采樣程序如下：

這個函數(shù)的輸入是機器人的當前位姿pnew和上一時刻的機器人的位姿pold，

OrientedPoint 
MotionModel::drawFromMotion(const OrientedPoint& p, const OrientedPoint& pnew, const OrientedPoint& pold) const{double sxy=0.3*srr;  //srr我理解為兩輪子里程計的方差OrientedPoint delta=absoluteDifference(pnew, pold); //具體如下面的介紹OrientedPoint noisypoint(delta);//存儲噪聲估計noisypoint.x+=sampleGaussian(srr*fabs(delta.x)+str*fabs(delta.theta)+sxy*fabs(delta.y));noisypoint.y+=sampleGaussian(srr*fabs(delta.y)+str*fabs(delta.theta)+sxy*fabs(delta.x));noisypoint.theta+=sampleGaussian(stt*fabs(delta.theta)+srt*sqrt(delta.x*delta.x+delta.y*delta.y));noisypoint.theta=fmod(noisypoint.theta, 2*M_PI);if (noisypoint.theta>M_PI)noisypoint.theta-=2*M_PI;return absoluteSum(p,noisypoint);
}

首先解釋一下函數(shù)：

OrientedPoint delta=absoluteDifference(pnew,pold);double 

具體的 內(nèi)容如下

orientedpoint<T,A> absoluteDifference(const orientedpoint<T,A>& p1,const orientedpoint<T,A>& p2){orientedpoint<T,A> delta=p1-p2;delta.theta=atan2(sin(delta.theta), cos(delta.theta));double s=sin(p2.theta), c=cos(p2.theta);return orientedpoint<T,A>(c*delta.x+s*delta.y, -s*delta.x+c*delta.y, delta.theta);
}

就是計算位姿的變化量，這個OrientedPoint delta的計算 結(jié)果對應(yīng)的理論公式的結(jié)果就是

???(x′?x)cosθ+(y′?y)sinθ?(x′?x)sinθ+(y′?y)cosθΔθ???
計算新舊幀的絕對誤差.具體就不再深入。
那么對于其中的三行代碼是分別給位姿的三個兩添加噪聲進去，為什么要這樣寫呢？

noisypoint.x+=sampleGaussian(srr*fabs(delta.x)+str*fabs(delta.theta)+sxy*fabs(delta.y));noisypoint.y+=sampleGaussian(srr*fabs(delta.y)+str*fabs(delta.theta)+sxy*fabs(delta.x));noisypoint.theta+=sampleGaussian(stt*fabs(delta.theta)+srt*sqrt(delta.x*delta.x+delta.y*delta.y));

首先我們對sampleGaussian（b2）函數(shù)已經(jīng)是有了解了，這個意思就是以均值為0 方差為b2的近似的正態(tài)分布的采樣算法。所以這些括號里的都是計算方差的，為什么參數(shù)不同呢？仔細看看就能找到規(guī)律了，但是明白之前的一篇博客里是有對srr srt str stt這些高斯模型參數(shù)的設(shè)置的，分別代表delta的三個變量之間的方差而已。
接下來的程序

noisypoint.theta=fmod(noisypoint.theta, 2*M_PI);if (noisypoint.theta>M_PI)noisypoint.theta-=2*M_PI;

百度一下fmod()函數(shù)是對浮點型數(shù)據(jù)進行取模運算，就是計算
noisypoint.theta/2*M_PI的余數(shù)。 因為要把角度差限定在[?π?π]之間。
最后是返回累計的噪聲：
absoluteSum(p,noisypoint);
這個函數(shù)的實現(xiàn)代碼是

template <class T, class A>
orientedpoint<T,A> absoluteSum(const orientedpoint<T,A>& p1,const orientedpoint<T,A>& p2){double s=sin(p1.theta), c=cos(p1.theta);return orientedpoint<T,A>(c*p2.x-s*p2.y,s*p2.x+c*p2.y, p2.theta) + p1;

P1是當前機器人的位姿，前面的c*p2.x-s*p2.y, s*p2.x+c*p2.y, p2.theta是分別在P1的位姿上加上各自的噪聲分量。

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

總結(jié)

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

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