MachinePerceptionandInteractionGroup(MPIG)gwt@

基于二維激光的機器人室內構圖宮婉婷MPIGOpenSeminar

0095GMapping算法理論研究GMapping代碼介紹GMapping算法原理分析Libpointmatcher代替ICP運行結果對比內容概要粒子濾波粒子濾波算法源于Montecarlo思想，即以某事件出現的頻率來指代該事件的概率。因此在濾波過程中，需要用到概率如P(x)的地方，一概對變量x采樣，以大量采樣及其相應的權值來近似表示P(x)。而Karman則只能處理線性高斯分布的概率問題論文理論研究

機器人圖模型通過RBPF濾波器構圖

SIR濾波流程弊端：在新觀測到臨時，每次評估抓取粒子的權重。當隨時間增長軌跡長度增加時，這些流程的精確性將會失效。引入：Doucetetal.使用遞歸的方程計算權重，限制提議分布滿足給定假設：許多存在的粒子濾波嚴格依賴與公式（6），然而通用的算法指定用于計算地圖的具體框架，并不確定提議分布怎樣計算與何時執行重采樣。，Gmapping算法描述了怎樣計算精確的提議分布，且進行適應性的重采樣。算法介紹：GMapping是一種改進的Rao-Blackwellized粒子濾波法，采樣的每個粒子都包含環境地圖信息。關鍵問題是怎樣降低粒子數。在ROS中是一個第三方包，包含openSLAM的Gmapping的一個ROS封裝。Gmapping的ROS節點slam_gmapping提供基于激光的SLAM，依靠移動機器人的激光和位姿數據，使用slam_gmapping創建2D網格地圖。貢獻：1.采用adaptivetechniques降低RBPF的粒子數，執行選擇性重采樣，限制粒子耗盡問題。2.提出方法計算精確的建議式分布，同時考慮機器人運動和最近觀測信息GMapping算法精確的提議分布提議分布π：一種對于目標分布的近似模擬目標分布：理想的粒子分布形態作用：作用是預測階段從提議分布π抽樣獲取下一批粒子，同時用來計算權重，用來理論上代替目標分布。直觀上，提議分布越近似目標分布，濾波器性能越好，理想情況下不需要重采樣步驟，（目標分布與提議分布差距太大的情況會引入重采樣）在SLAM中閉合后驗是不能得到的。模擬提議分布的多種方法里程運動模型，權重由觀測模型計算優點：運動模型易于計算，可以根據觀測模型計算權重，缺點：當傳感器信息精度遠高于基于里程計的機器人運動估測的情況下是次優的。由于僅有一部分樣本覆蓋了觀測模型下具有高可能性的狀態空間，所以每次獨立采樣的權重差異很大，需要相當多的樣本來充分覆蓋具有高觀測概率的區域。圖1示出了其中傳感器觀察似然的有意義的區域顯著小于運動模型的有意義的區域的情況。所以需要大量粒子來充分覆蓋具有高觀察可能性的區域

有效計算改進的提議分布

這樣，獲得一個最優分布的近似閉合形式，使用這個分布可以計算權重：AdaptiveResamplingGMapping采用Neff決定是否執行采樣步驟，每當采樣粒子數下降到N/2的閾值以下時，進行重新采樣，其中N是粒子數。大量實驗表明，這種方法大大降低了好粒子被替換的風險，因而降低重采樣操作次數，僅在需要時進行。算法流程代碼分析運動模型更新t時刻的粒子群（模型中添加了高斯噪聲）OrientedPointMotionModel::drawFromMotion(constOrientedPoint&p,constOrientedPoint&pnew,constOrientedPoint&pold)const{doublesxy=0.3*srr;OrientedPointdelta=absoluteDifference(pnew,pold);OrientedPointnoisypoint(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;returnabsoluteSum(p,noisypoint);//疊加噪音}掃描匹配/**Justscanmatcheverysingleparticle.Ifthescanmatchingfails,theparticlegetsadefaultlikelihood.*/inlinevoidGridSlamProcessor::scanMatch(constdouble*plainReading){//sampleanewposefromeachscaninthereferencedoublesumScore=0;for(ParticleVector::iteratorit=m_particles.begin();it!=m_particles.end();it++){OrientedPointcorrected;doublescore,l,s;score=m_matcher.optimize(corrected,it->map,it->pose,plainReading);//it->pose=corrected;if(score>m_minimumScore){it->pose=corrected;}else{if(m_infoStream){m_infoStream<<"ScanMatchingFailed,usingodometry.Likelihood="<<l<<std::endl;m_infoStream<<"lp:"<<m_lastPartPose.x<<""<<m_lastPartPose.y<<""<<m_lastPartPose.theta<<std::endl;m_infoStream<<"op:"<<m_odoPose.x<<""<<m_odoPose.y<<""<<m_odoPose.theta<<std::endl;}}m_matcher.likelihoodAndScore(s,l,it->map,it->pose,plainReading);sumScore+=score;it->weight+=l;it->weightSum+=l;//setuptheselectivecopyoftheactivearea//bydetachingtheareasthatwillbeupdatedm_matcher.invalidateActiveArea();m_puteActiveArea(it->map,it->pose,plainReading);}if(m_infoStream)m_infoStream<<"AverageScanMatchingScore="<<sumScore/m_particles.size()<<std::endl;}GridSlamProcessor::scanMatchPropasalDistributionRBPF-gmapping使用的是運動模型作為提議分布。GMapping中混合了運動模型和觀測模型的提議分布，將掃描觀測值整合到了提議分布中。因此均值和方差的計算與單純使用運動模型作為提議分布的有所區別。提議分布的作用是獲得最優的粒子點，同時用來計算權重，這個體現在ScanMatcher::likelihoodAndScore()和ScanMatcher::score()方法中，score方法中采用的是服從0均值的高斯分布近似提議分布,由于L(i)區域小的原理，所以采用混合的提議分布。權重更新voidGridSlamProcessor::updateTreeWeights(boolweightsAlreadyNormalized){if(!weightsAlreadyNormalized){normalize();//歸一化權重計算（20）}resetTree();//初始化粒子的樹節點，權重，訪問次數，父節點等propagateWeights();//為了迭代計算，計算上一次粒子的權重（19）}重采樣

算法結構分析硬件需求：使用slam_gmapping,需要移動機器人提供里程計數據，且水平安裝固定激光測距儀。Slam_gmapping節點作用：掃描輸入數據數據Odom里程計Tf坐標系Tf坐標轉換需要涉及激光坐標系，基坐標系和里程坐標系從激光掃描創建地圖Gmapping訂閱及發布話題slam_gmapping采用sensor_msg/LaserScan消息，并且構建地圖nv_msgs/OccupancyGrid)。地圖可以通過topic或service檢索。Slam_gmapping節點訂閱發布話題TFScanMap_metadatamap~entropy從這個話題獲得地圖數據，此話題鎖存，并定期更新機器人位姿分布熵表示服務Dynamic_map調用這個服務來獲取地圖數據參數Max_Urangeparticles

Max_interationsmap_frameOdom_frameminimumSorellsanplerangesrrMap_updatelskip需要的tf變換：<the

frame

attached

to

incoming

scans>

→

base_link通常是一個固定值，

由robot_state_publisher或

tf

static_transform_publisher.定期廣播。base_link

→

odom通常有里程計系統提供(例如，移動機器人的驅動)提供的tf變換：map

→

odom在地圖坐標系中機器人位姿的當前估計TF介紹：tf是一個包，可以使用戶隨著時間的推移跟蹤多個坐標系間的轉換，tf保持在時間緩沖區內的樹結構，可以使用戶在任意時間轉換兩個坐標系之間的點與矢量等。Tf的功能：機器人系統有許多3D坐標系，隨時間變換，比如worldframe,base_frame,這些tf可以隨時間跟蹤這些坐標系，可以給出確定時間點兩個坐標系之間的相對運動關系，以及基于某坐標系下物體的位姿，以及在map坐標系下base_frame坐標系的當前位姿。利用libpointmatcher替換ICP匹配經典的ICP算法Libpontmatcher介紹Libpontmatcher的ICP算法Libpointmatcher的編譯Libpointmatcher怎樣鏈接到外部工程經典ICP算法

ICP算法流程Libpontmatcher介紹Libpointmatcher是一個實現ICP算法來配準點云的庫，一種模塊化icp鏈，模塊化和快速性，l為配準算法提供了理想的解決方案。支持點到點和點到面的ICP配準。原始ICP算法的弊端：icp算法的本質是局部收斂近似算法，結果取決于初始姿態，需要提供初始假設，初始假設通常由慣性測量，里程計或啟發式運動模型提供，這些都有有限精度和觀測中隨時間增加的不確定度。另外激光掃描匹配的一個難點是大量的離群點，有大量的不能反映掃描特征的點。為了弱化這些因素對ICP算法性能的影響，引入Libpointmatcher替代原始ICPlibpointmatcher中,每一個模塊都是一個類,每一個類會定義一些自己的控制參數,所以使用一個yaml文件來輸入控制參數.ICP的輸入為兩個點云,輸出是兩個點云之間的旋轉量和平移變換量的估計值.一般稱第一個點云叫做參考點云(reference),第二叫reading(怎么讀取點云).ICP鏈首先會對兩幅點云做濾波處理(PointMatcher::DataPointsFilters),然后對兩幅點云進行匹配(PointMatcher::Matcher),去掉outlier點(PointMatcher::OutlierFilters),計算使配準誤差最小(PointMatcher::ErrorMinimizer)的變換參數(PointMatcher::TransformationParameters).編譯方法：http://libpointmatcher.readthedocs.io/en/latest/Compilation/Libpintmatcher的ICPLibpointmatcher怎樣鏈接到外部工程由于libpointmatche