使用LIDAR扫描实现在线同时定位和映射（SLAM）

这个示例使用:

这个例子演示了如何实现同步定位和测绘(SLAM)算法从模拟环境中获得的激光雷达扫描使用位姿图优化。这个例子需要Simulink®3D动画™和导万博1manbetx航工具箱™。

本示例的目标是使用激光雷达扫描并检索机器人的轨迹，在循环中使用机器人模拟器来构建环境地图。

可以在SLAM算法的基础上找到使用激光雷达扫描实现同步定位和绘图(SLAM)的例子。

文件中机器人的加载轨迹

机器人轨迹是给定给机器人在模拟环境中运动的路径点。对于本例，为您提供了机器人轨迹。

加载slamRobotTrajectory.mat

为说明目的，提供了该机器人的平面图和近似路径。这幅图像显示了被映射的环境和机器人的近似轨迹。

加载和查看虚拟世界

此示例使用具有两个车辆的虚拟场景，以及四个墙壁作为障碍物和配备Simulink 3D动画查看器中所示的LIDAR扫描仪的机器人。万博1manbetx您可以使用菜单栏，工具栏，导航面板，鼠标和键盘在虚拟场景中导航。观看者的主要特征在于平面操纵使用空间鼠标MATLAB®对象(万博1manbetx模型3 d动画)的例子。

创建并打开vrworld对象。

w = vrworld ('slamsimulatedworld.x3d');打开(w)

创建一个显示虚拟场景的图形

多联机= vrfigure (w)

VRF = VRFIGURE对象：带LIDAR传感器的1比1差动轮式机器人

初始化机器人在虚拟世界中的位置和旋转

虚拟场景表示为Simulink 3D动画使用的VRML文件的层次结构。万博1manbetx子对象的位置和方向是相对于父对象的。机器人vrnode用于操纵机器人在虚拟场景中的位置和方向。

要访问VRML节点，请使用适当的vrnode对象。节点通过它的名称和它所属的世界进行标识。

创建vrnode在虚拟环境中处理机器人。

robotVRNode = vrnode (w,“机器人”);

将机器人的初始位置设置为轨迹第一点，并将初始旋转设置为大约Y轴。

robotVRNode.children.translation = [trajectory(1,1) 0 trajectory(1,2)];robotVRNode.children.rotation = [0 1 0 0];

通过创建vrnode为机器人的激光雷达传感器创建句柄。

lidarvrnode = vrnode（w，'lidar_sensor');

模拟激光雷达共使用240条激光线，线与线的夹角为1.5度。

角= 180:-1.5:-178.5;角函数=(角度)”;

等待更新和初始化虚拟场景

暂停(1)

创建LIDAR SLAM对象

创建一个lidarSLAM目标和设置地图分辨率和最大激光雷达距离。本例使用一个模拟的虚拟环境。这个机器人vrworld有一个LIDAR传感器，范围为0到10米。将MAX LIDAR范围（8M）设置为小于最大扫描范围，因为激光读数在最大范围内的较低准确性。将网格图分辨率设置为每米的20个单元格，这给出了5厘米的精度。在整个示例中使用这两个参数。

maxLidarRange = 8;mapResolution = 20;slamAlg = lidarSLAM (mapResolution maxLidarRange);

循环闭合参数凭经验设置。使用较高的循环闭合阈值有助于抑制环路关闭识别过程中的误报。请记住，高分匹配可能仍然是一个糟糕的匹配。例如，在具有类似或重复特征的环境中收集的扫描更有可能产生假阳性。使用较高的循环闭合搜索RADIUS允许该算法在当前姿势估计周围搜索循环闭合的电流介质估计的更宽范围。

Slamalg.LoopClosurethreshold = 200;Slamalg.LoopClosuresearchRadius = 3;ControlRate = RateControl（10）;

观察循环闭合和优化过程的影响

创建一个循环以通过虚拟场景导航机器人。机器人位置在轨迹点中的循环中更新。扫描从机器人获得，因为机器人通过环境导航。

当机器人移动时，会自动检测到环路闭包。当检测到一个闭环时，就执行位姿图优化。这可以通过输出进行检查优化info.isperformed.价值addScan。

当识别出第一环闭环并在视觉上验证结果时，显示快照以演示扫描和姿势。该图显示了第一环闭合的覆盖扫描和优化的姿势图。

在收集和处理所有扫描后，将呈现最终构建的地图。

随着机器人在虚拟场景中导航，情节不断更新

firstloopclosure = false;扫描=单元格（长度（轨迹），1）;数字为了i = 1：长度（轨迹）%使用平移属性移动机器人。robotvrnode.children.translation = [轨迹（i，1）0轨迹（i，2）];Vrdrawnow;％读取从机器人的LIDAR传感器获得的范围读数。范围= lidarvrnode.pickedrange;如果对象是的，模拟的激光雷达读数将提供-1值％ 超出范围。使所有这些值大于% maxLidarRange。范围（范围==  -  1）= MaxLidArrange + 2;％从范围和角度创建一个lidarscan对象。扫描{i} = lidarscan（范围，角度）;[iscanacceped，loopclosentinfo，优化info] = addscan（斜坡，扫描{i}）;如果iscanacceped%可视化扫描绘图和姿态如何更新作为机器人导航％通过虚拟场景展示（史册）;%显示第一个检测到的循环闭包％FirstLoopClosuple标志用于捕获第一个循环闭合事件如果优化info.isperformed &&〜firstloopclosure firstloopclosure = true;展示（斜坡，'姿势'那“关闭”);抓住在;显示(slamAlg.PoseGraph);抓住从;标题(“第一循环关闭”);snapnow结尾结尾等待（ControlRate）;结尾

在所有扫描被添加到斜坡对象。

展示（斜坡，'姿势'那“关闭”);抓住在显示(slamAlg.PoseGraph);抓住从标题({'环境的最终建造地图'那“机器人的轨迹”}）;

构建占用网格图

优化的扫描和姿势可用于生成一个占领子它将环境表示为一个概率占用网格。

[扫描，优化] = ScanSandpose（Slamalg）;地图= buildmap（扫描，优化，mapresolution，maxlidarrange）;

可视化使用激光扫描和优化的姿态图填充的占用网格地图。

数字;显示地图）;抓住在显示(slamAlg。PoseGraph,“id”那“关闭”);抓住从标题('占用网格地图使用LIDAR Slam'建造');

关闭虚拟场景。

关闭(多联机);关闭(w);删除(w);