利用扫描匹配估计机器人姿态
这个例子演示了如何使用正态分布变换(NDT)算法[1]匹配两个激光扫描。扫描匹配的目标是找到在进行扫描的两个机器人位置之间的相对姿态(或变换)。扫描可以根据重叠特征的形状进行对齐。
为了估计这一姿态，NDT将激光扫描细分为二维单元，每个单元都分配了相应的正态分布。分布表示在该单元格中测量一个点的概率。一旦概率密度计算，优化方法找到当前激光扫描和参考激光扫描之间的相对姿态。为了加快算法的收敛速度，可以提供初始的姿态猜测。通常情况下，机器人里程表被用来提供初始估计。
如果将扫描匹配应用于扫描序列，则可以使用它来恢复机器人所遍历的环境的粗略地图。扫描匹配在其他应用中也起着至关重要的作用，如位置跟踪和同步定位与映射(SLAM)。
从文件加载激光扫描数据
负载lidarScans.mat
激光扫描数据由移动机器人在室内环境中采集。该区域的大致平面图，以及机器人在空间中的路径，如下图所示。
图二激光扫描
选择两个激光扫描来进行扫描匹配lidarScans．它们应该在序列中靠得很近，具有共同的特征。
referenceScan = lidarScans(180);currenscan = lidarScans(202);
显示两个扫描结果。注意有平移和旋转偏移，但一些特征仍然匹配。
currScanCart = currentScan.Cartesian;refScanCart = referencescan .笛卡尔;图绘制(refScanCart (: 1), refScanCart (:, 2),“k”。）;持有在情节(currScanCart (: 1) currScanCart (:, 2),“r”。）;传奇(“参考激光扫描”，“当前激光扫描”，“位置”，“西北”）;
运行扫描匹配算法和显示转换扫描
将这两个扫描传递给扫描匹配函数。matchScans计算当前扫描相对于参考扫描的相对姿态。
transform = matchScans(当前扫描，引用扫描)
变换=1×30.5348 -0.0065 -0.0336
为了直观地验证相对姿态计算正确，使用将当前扫描转换为计算出的姿态transformScan．这种转换后的激光扫描可用于可视化结果。
transScan = transformScan(当前扫描，转换);
在变换后的电流激光扫描旁边显示参考扫描。如果扫描匹配成功，则两个扫描应该对齐良好。
图绘制(refScanCart (: 1), refScanCart (:, 2),“k”。）;持有在transScanCart = transScan.Cartesian;情节(transScanCart (: 1) transScanCart (:, 2),“r”。）;传奇(“参考激光扫描”，“转换电流激光扫描”，“位置”，“西北”）;
使用迭代扫描匹配构建占用网格地图
如果将扫描匹配应用于扫描序列，则可以使用它来恢复环境的粗略映射。使用occupancyMap类来构建环境的概率占用网格图。
为15米乘15米的区域创建一个占用网格对象。设置地图的原点为[-7.5 -7.5]。
map = occuancymap (15,15,20);地图。GridLocationInWorld = [-7.5 -7.5]
map = occuancymap with properties: mapLayer properties OccupiedThreshold: 0.6500 FreeThreshold: 0.2000 probabilitys饱和度:[0.0010 0.9990]LayerName: 'probabilityLayer' DataType: 'double' DefaultValue: 0.5000 GridLocationInWorld: [-7.5000 -7.5000] GridOriginInLocal: [0 0] LocalOriginInWorld:[-7.5000 -7.5000]分辨率:20 GridSize: [300 300] XLocalLimits: [0 15] YLocalLimits: [0 15] 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;transform = initialPose;
创建一个循环来处理扫描和映射该区域。激光扫描是成对进行的。第一次扫描定义为参考扫描，第二次扫描定义为当前扫描。然后将两个扫描传递给扫描匹配算法，并计算两个扫描之间的相对姿态。的exampleHelperComposeTransform函数用于计算机器人的累积绝对位姿。然后，扫描数据和机器人的绝对姿势可以传递到insertRay占用网格的功能。
遍历所有扫描并计算它们之间的相对姿势。为idx = 2:numScans%成对处理数据。referenceScan = lidarScans(idx-1);currenscan = lidarScans(idx);运行扫描匹配。注意，扫描角度保持不变%不需要重新计算。要提高精度，请设置最大值%迭代次数到500。使用最后一个变换%迭代作为初始估计。[transform,stats] = matchScans(currentScan,referenceScan，.．.“MaxIterations”, 500,“InitialPose”、转换);统计结构中的|分数|是一个很好的指示%扫描匹配的质量。如果统计数据。Score / currentScan。计数< 1.0 disp([“索引扫描匹配分数低”num2str (idx)”。分数= 'num2str (stats.Score)“。”]);结束维护机器人姿势列表。absolutePose = exampleHelperComposeTransform(poseList(idx-1，:)，transform);poseList(idx，:) = absolutePose;%将当前激光扫描整合到概率占用中%的网格。insertRay(地图,absolutePose currentScan 10);结束
可视化地图
可视化使用激光扫描填充的占用网格地图。
图显示(地图);标题(“使用扫描匹配结果构建占用网格图”）;
绘制扫描匹配算法计算出的机器人绝对位姿。这显示了机器人在环境地图上走过的路径。
持有在情节(poseList (: 1) poseList (:, 2),“波”，“DisplayName的”，“估计机器人位置”）;传奇(“显示”，“位置”，“西北”）
参考文献
[1] P. Biber, W. Strasser，“正态分布变换:激光扫描匹配的新方法”，IEEE/RSJ智能机器人与系统国际会议论文集(IROS)， 2003年，第2743-2748页
您也可以从以下列表中选择一个网站: