gmapping文庫(kù)

1、gmapping(fastslam)1、 獲取激光數(shù)據(jù)并處理如果是第一個(gè)激光數(shù)據(jù)，則初始化粒子，每個(gè)粒子維護(hù)一幅地圖map，機(jī)器人的位姿pose，上個(gè)時(shí)刻的位姿previouspose，權(quán)重weight，權(quán)重總和weightsum。地圖包括的信息有：m_center：地圖中心點(diǎn)（x, y）、m_worldsizex：地圖長(zhǎng)度、m_worldsizey：地圖寬度、m_delta：網(wǎng)格邊長(zhǎng)代表的長(zhǎng)度。單位 m/cell m_xmin、m_xmax、m_ymin、m_ymax，分別對(duì)應(yīng)地圖橫坐標(biāo)最小值和最大值、縱坐標(biāo)最小值和最大值。粒子初始化，初始信息都相同tnode* node = new tno

2、de(initialpose, 0, 0, 0);scanmatchermap lmap(point(xmin + xmax, ymin + ymax) * .5, xmax - xmin,ymax - ymin, delta);for (unsigned int i = 0; i size; i+) m_particles.push_back(particle(lmap);m_particles.back().pose = initialpose;m_particles.back().previouspose = initialpose;m_particles.back().setweigh

3、t(0);m_particles.back().previousindex = 0;/ this is not needed/ m_particles.back().node=new tnode(initialpose, 0, node, 0);/ we use the root directlym_particles.back().node = node;geometry_msgs/twist.msg# this expresses velocity in free space broken into its linear and angular parts.vector3 linearve

4、ctor3 angular/ 獲取激光在里程計(jì)坐標(biāo)系上的坐標(biāo)tf:stamped odom_pose; try tf_.transformpose(odom_frame_, centered_laser_pose_, odom_pose); 處理激光數(shù)據(jù)gridslamprocessor:processscan(const rangereading & reading, int adaptparticles)對(duì)于第一條數(shù)據(jù)：1、 計(jì)算激光在每個(gè)粒子相應(yīng)的位姿所掃描到的區(qū)域scanmatcher:computeactivearea(scanmatchermap& map, const orien

5、tedpoint& p, const double* readings)對(duì)每一束激光束，設(shè)start為該激光束起點(diǎn)，end為激光束端點(diǎn)（障礙物位置），使用bresenham劃線算法確定激光束經(jīng)過(guò)的網(wǎng)格，算法原理如下：設(shè)start、end所在直線方程為y = kx + b，首先通過(guò)直線的斜率確定了在x方向進(jìn)行單位步進(jìn)還是y方向進(jìn)行單位步進(jìn)：當(dāng)斜率k的絕對(duì)值|k|1時(shí)，在y方向進(jìn)行單位步進(jìn)。 下面以|k|1時(shí)推導(dǎo)bresenham算法的數(shù)學(xué)依據(jù)： 如上圖，已知有一直線y = kx+b，|k|1。我們通過(guò)斜率確定了x方向?yàn)閱挝徊竭M(jìn)。當(dāng)x = 時(shí)，y = 。那么當(dāng)x 執(zhí)行一個(gè)單位步進(jìn)時(shí)（即x = +1

6、時(shí)）,y等于還是等于+1更符合這個(gè)直線方程呢？單憑肉眼我們很難得出結(jié)論，最好的辦法當(dāng)然是比較和+1和真實(shí)的方程的y值的差是多少（即yreal = k*(+1)+b），看看哪一個(gè)更靠近真實(shí)的方程的y值。 我們?cè)O(shè) dupper = + 1 - yreal = +1 - k*(+ 1)+b）; 表示+1和方程真實(shí)值的差 ddown = yreal - = k*(+1)+b）- ; 表示和方程真實(shí)值的差 那就是我們要比較dupper和ddown的大小。假設(shè)diff = dupper - ddown = （+1 - k*(+1)+b） - （k*(+1)+b）- ） 令x 為線段x方向的間距（即|end

7、.x - start.x|），y 為線段y方向的間距（|end.y - start.y|）。= x* diff = 2*x* -2*y* -2*y-x*（2b-1）；那么 = +2*x*（- ）-2*y； = + 2*x - 2*y （ 0）其中 - 取0還是1，取決于的符號(hào), 0取0，算法循環(huán)判斷，如果小于0，則下一個(gè)點(diǎn)的y坐標(biāo)加1。 根據(jù)等式diff = dupper - ddown = （+1 - （k*(+1)+b） - （k*(+1)+b）- ）以及k = y/x，我們可以得出起始像素（x0,y0）的參數(shù)p0的值：p0 =2*x* -2*y* -2*y-x*（2b-1）= x-2*y

8、； void gridlinetraversal:gridlinecore( intpoint start, intpoint end, gridlinetraversalline *line )/ d相當(dāng)于 int dx, dy, incr1, incr2, d, x, y, xend, yend, xdirflag, ydirflag; int cnt = 0; dx = abs(end.x-start.x); dy = abs(end.y-start.y); if (dy end.x) x = end.x; y = end.y; ydirflag = (-1); xend = start

9、.x; else x = start.x; y = start.y; ydirflag = 1; xend = end.x; line-pointscnt.x=x; line-pointscnt.y=y; cnt+; if (end.y - start.y) * ydirflag) 0) while (x 0) d+=incr1; else y+; d+=incr2;line-pointscnt.x=x;line-pointscnt.y=y;cnt+; else while (x 0) d+=incr1; else y-; d+=incr2;line-pointscnt.x=x;line-po

10、intscnt.y=y;cnt+; else d = dy - 2*dx; incr1 = -2*dx; incr2 = 2 * (dy - dx); if (start.y end.y) y = end.y; x = end.x; yend = start.y; xdirflag = (-1); else y = start.y; x = start.x; yend = end.y; xdirflag = 1; line-pointscnt.x=x; line-pointscnt.y=y; cnt+; if (end.x - start.x) * xdirflag) 0) while (y

11、0) d+=incr1; else x+; d+=incr2;line-pointscnt.x=x;line-pointscnt.y=y;cnt+; else while (y 0) d+=incr1; else x-; d+=incr2;line-pointscnt.x=x;line-pointscnt.y=y;cnt+; line-num_points = cnt;2、 更新單元格被掃描過(guò)的總次數(shù)visits和被標(biāo)記為障礙物的次數(shù)n，scanmatcher:registerscan(scanmatchermap& map, const orientedpoint& p, const dou

12、ble* readings)/ 匹配掃描根據(jù)bresenham算法確定激光束掃描過(guò)的單元格，所以掃描過(guò)的單元格被掃描過(guò)的次數(shù)加1，激光束末端對(duì)應(yīng)的單元格障礙物標(biāo)記次數(shù)加1，并累加障礙物坐標(biāo)acc.x和acc.y，則障礙物的。最后單元格是障礙物的概率p = n / visits。后續(xù)激光處理后續(xù)的激光數(shù)據(jù)要與有地形進(jìn)行匹配，修正每個(gè)粒子的位置，對(duì)應(yīng)的函數(shù)為gridslamprocessor:scanmatch(const double* plainreading) 首先計(jì)算當(dāng)前位置initpose的一個(gè)分?jǐn)?shù)score：計(jì)算每束激光對(duì)應(yīng)的障礙物坐標(biāo)phit，再計(jì)算phit對(duì)應(yīng)的網(wǎng)格坐標(biāo)iphit，

13、激光束上與障礙物相鄰的非障礙物網(wǎng)格為pfree，pfree的坐標(biāo)由phit移動(dòng)一個(gè)網(wǎng)格得到；然后在iphit以及周圍的8個(gè)網(wǎng)格搜索最有可能是障礙物的網(wǎng)格。最有可能的判斷方法為：該網(wǎng)格是障礙物的概率大于一個(gè)閾值，其對(duì)應(yīng)的pfree是障礙物的概率小于一個(gè)閾值，并且該網(wǎng)格對(duì)應(yīng)的障礙物坐標(biāo)1.0/n * point(acc.x,acc.y)與phit的距離d最小；最后score = score + exp(-1.0 / sigma * d * d)。累加計(jì)算score，可參考ndt(normal distributions transform)算法。因?yàn)榫嚯x越大，score應(yīng)越小，score較大值應(yīng)集

14、中在距離最小值處，這符合正態(tài)分布模型，所以使用exp來(lái)計(jì)算每束激光的score。接著對(duì)initpose進(jìn)行微調(diào)，即分別輕微調(diào)整initpose的坐標(biāo)和角度，計(jì)算其分?jǐn)?shù)，最后選擇分?jǐn)?shù)最大的對(duì)應(yīng)的位姿作為修正后的位姿。amcl(自適應(yīng)蒙特卡洛定位)測(cè)距儀測(cè)量模型：1、 光束模型（beam models）。1、該模型包含四種類型的誤差（噪聲）：較小的測(cè)量噪聲、動(dòng)態(tài)物體帶來(lái)的誤差、未探測(cè)到物體帶來(lái)的誤差（沒(méi)有探測(cè)到物體時(shí)將使用測(cè)距儀的最大測(cè)程作為測(cè)量數(shù)據(jù)，因此也有可能不正確）、隨機(jī)誤差。（1） 測(cè)量噪聲：由測(cè)距儀的精度、大氣對(duì)測(cè)量信號(hào)的影響等造成，其概率模型一般是以理想測(cè)量距離為均值的高斯模型，表示為

15、：是從在地圖上從位置沿激光束方向到障礙物的實(shí)際距離，是傳感器測(cè)量距離。是歸一化常數(shù)：（2） 動(dòng)態(tài)物體誤差：由未預(yù)測(cè)到的物體（unexpected objects）帶來(lái)的誤差。環(huán)境是動(dòng)態(tài)的，而保存的地圖是靜態(tài)的，即不變的。沒(méi)有包含在地圖里的物體的出現(xiàn)會(huì)產(chǎn)生一個(gè)令人意外的比較小的測(cè)量數(shù)據(jù)。典型的動(dòng)態(tài)物體就是行人。處理這些誤差的一種方法就是把它當(dāng)成傳感器噪聲。測(cè)量到的距離越大，檢測(cè)到動(dòng)態(tài)物體的概率越小，且小距離對(duì)應(yīng)的測(cè)量到的是動(dòng)態(tài)物體的概率應(yīng)遠(yuǎn)大于大距離的概率，隨意概率隨距離的增大呈指數(shù)下降趨勢(shì)，所以其概率模型為指數(shù)分布，表示為：（3） 測(cè)量失?。河袝r(shí)候，傳感器探測(cè)不到障礙物，比如試圖探測(cè)一些吸收光線的物體，此時(shí)的探測(cè)數(shù)據(jù)將失