估计机器人位姿与扫描匹配
这个案例展示了如何使用正态分布匹配两个激光扫描变换(无损检测)算法[1]。扫描匹配的目标是找到两者之间的相对的姿势(或变换)机器人位置的扫描。扫描可以对齐基于重叠的形状特征。
估计这个姿势,无损检测将激光扫描成2 d细胞,每个细胞是分配相应的正态分布。是测量一个点的概率分布的细胞。一旦计算概率密度,优化方法找到之间的相对构成当前激光扫描和参考激光扫描。加快收敛的方法,提出可以提供的初始猜测。通常,机器人测程法用于供应最初的估计。
如果你应用扫描匹配的序列扫描,您可以使用它来恢复一个地图的大致环境,机器人遍历。扫描匹配也在其他应用程序中起着至关重要的作用,如位置跟踪和同步定位和映射(大满贯)。
激光扫描数据加载文件
负载lidarScans.mat
激光扫描数据被收集在一个室内环境中移动机器人。区域的近似平面布置图以及通过空间机器人的路径,如下图所示。
两个激光扫描图
选择两个激光扫描扫描匹配lidarScans。他们应该有共同的特性被紧密的序列。
referenceScan = lidarScans (180);currentScan = lidarScans (202);
显示两个扫描。注意有平移和旋转补偿,但仍有一些特征匹配。
currScanCart = currentScan.Cartesian;refScanCart = referenceScan.Cartesian;图绘制(refScanCart (: 1), refScanCart (:, 2),“k”。);持有在情节(currScanCart (: 1) currScanCart (:, 2),“r”。);传奇(“参考激光扫描”,目前激光扫描的,“位置”,“西北”);
扫描匹配算法和显示转换扫描运行
通过这两个扫描扫描匹配函数。matchScans计算的相对构成当前扫描对参考扫描。
变换= matchScans (currentScan referenceScan)
变换=1×30.5348 -0.0065 -0.0336
直观地验证相对姿态计算正确,改变当前扫描的计算使用transformScan。这改变了激光扫描可用于可视化结果。
transScan = transformScan (currentScan、转换);
显示参考扫描当前激光扫描与转换。如果扫描匹配成功,两个扫描应该很好地结合。
图绘制(refScanCart (: 1), refScanCart (:, 2),“k”。);持有在transScanCart = transScan.Cartesian;情节(transScanCart (: 1) transScanCart (:, 2),“r”。);传奇(“参考激光扫描”,改变了目前激光扫描的,“位置”,“西北”);
使用迭代构建占用网格地图扫描匹配
如果你应用扫描匹配的序列扫描,您可以使用它来恢复环境的一个粗略的地图。使用occupancyMap类来建立一个概率占用网格环境的地图。
创建一个占用网格对象为15米,15米区域。设置地图的起源(-7.5 - -7.5)。
地图= occupancyMap (15 15 20);地图。GridLocationInWorld = (-7.5 - -7.5)
地图= occupancyMap属性:OccupiedThreshold mapLayer属性:0.6500 FreeThreshold: 0.2000 ProbabilitySaturation: [0.0010 - 0.9990] LayerName:“probabilityLayer”数据类型:“双”DefaultValue: 0.5000 GridLocationInWorld: [-7.5000 - -7.5000] GridOriginInLocal: [0 0] LocalOriginInWorld:[-7.5000 - -7.5000]决议:20 GridSize: 300年[300]XLocalLimits: 15 [0] YLocalLimits: 15 [0] XWorldLimits: [-7.5000 - 7.5000] YWorldLimits: [-7.5000 - 7.5000]
预先分配一个数组来捕捉机器人的绝对运动。初始化第一个冒充(0 0 0)。所有其他姿势是相对于第一个测量扫描。
numScans =元素个数(lidarScans);initialPose = (0 0 0);poseList = 0 (numScans, 3);:poseList (1) = initialPose;变换= initialPose;
创建一个循环处理扫描和映射。激光扫描处理成对。定义第一个扫描参考扫描和第二扫描电流扫描。两个扫描然后传递给扫描匹配算法,计算两者之间的相对姿态扫描。的exampleHelperComposeTransform函数是用来计算的累积绝对机器人位姿。扫描数据以及绝对的机器人位姿可以传递到insertRay占用网格的功能。
%循环扫描,计算它们之间的相对姿态为idx = 2: numScans成对%处理数据。referenceScan = lidarScans (idx-1);currentScan = lidarScans (idx);%运行扫描匹配。注意,扫描角度保持不变和做%不必重新计算。提高准确性,设置最大%的迭代次数为500。使用最后的变换%迭代的初始估计。(变换,数据)= matchScans (currentScan referenceScan,…“MaxIterations”,500,“InitialPose”、转换);% |积分|在统计数据的结构是一个很好的迹象%的扫描匹配质量。如果统计数据。分数/ currentScan。数< 1.0 disp ([“低扫描匹配分数指数”num2str (idx)”。分数= 'num2str (stats.Score)“。”]);结束%保持机器人构成的列表。absolutePose = exampleHelperComposeTransform (poseList (idx-1:),变换);poseList (idx:) = absolutePose;%将当前激光扫描集成到概率入住率%的网格。insertRay(地图,absolutePose currentScan 10);结束
可视化地图
可视化占用网格地图的激光扫描。
图显示(地图);标题(“占用网格地图使用扫描匹配结果”);
情节绝对机器人姿态的计算是通过扫描匹配算法。这显示了机器人的路径通过环境的地图了。
持有在情节(poseList (: 1) poseList (:, 2),“波”,“DisplayName的”,“估计机器人位置”);传奇(“显示”,“位置”,“西北”)
引用
[1]p . Biber w·摩根,“正态分布变换:激光扫描匹配的新方法,“学报IEEE / RSJ智能机器人和系统国际会议(——),2003年,页2743 - 2748
