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

此示例使用：

打开直播脚本

此示例演示了如何使用姿势图优化从模拟环境获得的LIDAR扫描上的同时定位和映射（SLAM）算法。此示例需要Simulink®3D动画™和导航金宝app工具箱™。

此示例的目标是使用LIDAR扫描构建环境的地图，并使用循环中的机器人模拟器检索机器人的轨迹。

可以在SLAM算法的基础上找到使用LIDAR扫描实现同时定位和映射（SLAM）例子。

从文件中加载机器人的轨迹

机器人轨迹是给出机器人的航点，以便在模拟环境中移动。对于此示例，为您提供了机器人轨迹。

加载slamrobottrajectory.mat.

提供了用于说明性目的的机器人的平面图和近似路径。此图像显示映射的环境以及机器人的近似轨迹。

加载和查看虚拟世界

此示例使用具有两个车辆的虚拟场景，以及四个墙壁作为障碍物和配备Simulink 3D动画查看器中所示的LIDAR扫描仪的机器人。金宝app您可以使用菜单栏，工具栏，导航面板，鼠标和键盘在虚拟场景中导航。观看者的主要特征在于使用Space MouseMatlab®对象的平面操作（金宝appSimulink 3D动画）例子。

创建和打开vrworld.目的。

w = vrworld（'slamsimulatedworld.x3d'）;开放（w）

创建一个数字，显示虚拟场景

VRF = VRFIGURE（W）

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

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

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

访问VRML节点，适当vrnode.必须创建对象。该节点由其名称和它所属的世界标识。

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

robotvrnode = vrnode（w，'机器人'）;

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

robotvrnode.children.translation = [轨迹（1,1）0轨迹（1,2）];robotvrnode.children.rotation = [0 1 0 0];

通过创建VRNode在机器人上创建LIDAR传感器的手柄。

lidarvrnode = vrnode（w，'lidar_sensor'）;

模拟的激光雷达使用总计240激光线，这些线之间的角度为1.5度。

角度= 180：-1.5：-178.5;角度= deg2rad（角度）';

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

暂停（1）

创建LIDAR SLAM对象

创建一个Lidarslam.对象并设置地图分辨率和最大LIDAR范围。此示例使用模拟虚拟环境。这个机器人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.70707apraph）;抓住离开;标题（'第一个循环关闭'）;Snapnow.结尾结尾等待（ControlRate）;结尾

绘制所有扫描后的最终构建的地图斜坡目的。

展示（斜坡，'姿势'那'离开'）;抓住上展示（Slamalg.70707apraph）;抓住离开标题（{'环境的最终建造地图'那'机器人的轨迹'}）;

构建占用网格图

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

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

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

数字;显示地图）;抓住上展示（Slamalg.707apraph，'ids'那'离开'）;抓住离开标题（'占用网格地图使用LIDAR Slam'建造'）;

关闭虚拟场景。

关闭（VRF）;关闭（w）;删除（w）;