连接到Gazebo的TurtleBot

首先，通过以下步骤在虚拟机的办公环境中生成一个模拟的TurtleBot开始使用凉亭和模拟乌龟机器人(ROS工具箱)启动露台的办公室世界从桌面，如下所示。

在主机上的MATLAB实例中，运行以下命令初始化MATLAB中的ROS全局节点，并通过其IP地址连接到虚拟机中的ROS主机ipaddress代替ipaddress在虚拟机中使用TurtleBot的IP地址。

ipaddress =“192.168.2.150”；rosinit (ipaddress, 11311);

使用NodeURI http://192.168.2.1:53461/初始化全局节点/matlab_global_node_73841

模拟办公环境布局:

加载模拟世界的地图

在Gazebo中加载办公环境的二进制占用网格。该地图是通过在办公环境中驾驶TurtleBot生成的。该地图使用Kinect®的距离方位读数和来自露台/模型州的话题。

负载officemap.mat显示（地图）

建立激光传感器模型和乌龟机器人运动模型

TurtleBot可以建模为差动驱动机器人，它的运动可以通过里程计数据估计。的噪音属性定义了机器人的旋转和直线运动的不确定性。增加了odometryModel。噪音当使用里程计测量传播粒子时，属性将允许更多的传播。指odometryMotionModel有关物业详情，请参阅。

odometryModel = odometryMotionModel;odometryModel。噪音= [0.2 0.2 0.2 0.2];

TurtleBot上的传感器是由Kinect读数转换而成的模拟测距仪。似然场方法用于通过比较测距仪测量的端点和占用图来计算感知一组测量的概率。如果端点与占用地图上的被占用点相匹配，则感知到这种测量值的概率较高。为了获得更好的测试结果，需要对传感器模型进行调整，使其与传感器的实际性能相匹配。房地产SensorLimits定义传感器读数的最小和最大范围。房地产地图定义用于计算似然域的占用图。请参考概率场模型有关物业详情，请参阅。

RangeFindModel=likelihoodFieldSensorModel；RangeFindModel.SensorLimits=[0.45 8]；RangeFindModel.Map=Map；

设置rangeFinderModel.SensorPose到固定摄像机相对于机器人基座的坐标变换。这是用来转换激光读数从相机框架到TurtleBot的基础框架。请参考在ROS中访问tf转换树(ROS工具箱)有关坐标转换的详细信息。

请注意,目前传感器模型只与固定在机器人框架上的传感器兼容，这意味着传感器变换是恒定的。

%查询ROS中的tf树。tftree=rostf；waitForTransform（tftree，“/base_link”,' / base_scan '）;sensorTransform = getTransform (tftree,“/base_link”,' / base_scan '）;得到欧拉旋转角度。(sensorTransform.Transform.Rotation laserQuat =。W sensorTransform.Transform.Rotation.X...传感器变换。变换。旋转。Y传感器变换。变换。旋转。Z]；laserRotation=quat2eul（laserQuat，“ZYX股票”）;%设置|SensorPose|，其中包括沿着base_link的翻译% +X， +Y方向在米和旋转角度沿base_link的+Z轴%以弧度表示。rangeFinderModel.SensorPose=...[sensorTransform.Transform.Translation.X sensorTransform.Transform.Translation.Y laserRotation（1）]；

接收传感器测量和发送速度命令

创建ROS订户，用于从TurtleBot检索传感器和里程测量数据。

laserSub = rossubscriber (“/扫描”）;odomSub = rossubscriber (' /奥多姆'）;

创建用于向TurtleBot发送速度命令的ROS发行者。TurtleBot订阅“/mobile_base/commands/velocity”用于速度命令。

[velPub, velMsg] =...rospublisher (' / cmd_vel ',geometry_msgs /“扭转操作”）;

初始化AMCL对象

实例化一个AMCL对象amcl．看到蒙特卡洛定位有关该课程的更多信息。

amcl=蒙特卡洛本地化；amcl.UseLidarScan=true；

分配运动模型和传感器模型属性amcl对象。

amcl.MotionModel=里程计模型；amcl.SensorModel=RangeFindModel；

粒子过滤器只在机器人的运动超过时更新粒子UpdateThresholds，定义[x, y，偏航]中的最小位移来触发滤波器更新。这可以防止由于传感器噪音而过于频繁的更新。粒子重采样发生在amcl。ResamplingInterval过滤器更新。使用更大的数值会导致更慢的粒子损耗，同时也以更慢的粒子收敛为代价。

amcl。UpdateThresholds = (0.2, 0.2, 0.2);amcl。ResamplingInterval = 1;

配置AMCL对象进行初始姿态估计定位。

amcl。ParticleLimits定义重采样过程中将生成的粒子数的下限和上限。允许生成更多粒子可能会提高收敛到真实机器人姿势的机会，但会影响计算速度，粒子可能需要更长的时间，甚至无法收敛。请参阅“KL-D采样”[1]中的g'节用于计算粒子数的合理界限值。请注意，与初始姿势估计的定位相比，全局定位可能需要更多的粒子。如果机器人知道其初始姿势时存在一些不确定性，则此类附加信息可以帮助AMCL以更少的粒子数更快地定位机器人，i、 e.您可以在中使用较小的上限值amcl。ParticleLimits．

现在设置amcl。GlobalLocalization错误并向AMCL提供一个估计的初始姿势。通过这样做，AMCL持有一个初始信念，即机器人的真实姿势遵循一个平均值等于的高斯分布amcl。InitialPose协方差矩阵等于初始协方差．最初的姿势估计应该根据你的设置来获得。这个示例助手从Gazebo检索机器人当前的真实姿态。

请参阅为全局本地化配置AMCL对象以获取关于使用全局本地化的示例。

amcl.ParticleLimits=[500 5000]；amcl.GlobalLocalization=false；amcl.InitialPose=ExampleHelperAMLGazebotruePose；amcl.InitialCovariance=eye（3）*0.5；

设置可视化和驱动乌龟机器人助手。

设置示例HelperAMCLV可视化以绘制地图并更新地图上机器人的估计姿势、粒子和激光扫描读数。

visualizationHelper = ExampleHelperAMCLVisualization(地图);

机器人运动是AMCL算法的关键。在本例中，我们使用ExampleHelperAMCLWanderer类随机驱动TurtleBot，该类在环境中驱动机器人，同时使用controllerVFH类。

wanderHelper =...ExampleHelperAMCLWanderer(laserSub, sensorTransform, velPub, velMsg);

本地化过程

当机器人在移动时，AMCL算法在每个时间步骤中更新里程计和传感器读数。在初始化粒子并绘制到图中之前，请允许几秒钟。在这个例子中，我们将运行numUpdatesAMCL更新。如果机器人不能收敛到正确的机器人姿态，可以考虑使用较大的numUpdates．

numUpdates = 60;我= 0;而我< numUpdates%接收激光扫描和里程计信息。scanMsg=receive（laserSub）；odompose=odomSub.LatestMessage；创建lidarScan对象以传递给AMCL对象。扫描=激光雷达扫描（扫描信息）；%对于倒置安装的传感器，您需要反转使用“翻转”功能读取扫描角度的%顺序。从测程信息中计算机器人的姿态[x,y，偏航]。(odompose.Pose.Pose.Orientation odomQuat =。W odompose.Pose.Pose.Orientation.X...odompose.Pose.Pose.Orientation。Y, odompose.Pose.Pose.Orientation.Z];odomRotation = quat2eul (odomQuat);构成= [odompose.Pose.Pose.Position。X, odompose.Pose.Pose.Position.Y odomRotation (1)];%更新估计的机器人姿态和协方差使用新的里程计和%传感器读数。[IsUpdate，estimatedPose，estimatedCovariance]=amcl（姿势，扫描）；驱动机器人到下一个姿势。漫步(wanderHelper);%在地图上绘制机器人的估计姿势、粒子和激光扫描。如果isUpdated i = i + 1; / /更新plotStep(visualizationHelper, amcl, estimatedPose, scan, i)结束结束

在MATLAB中停止TurtleBot和关闭ROS

停下来

使用NodeURI http://192.168.2.1:53461/关闭全局节点/matlab_global_node_73841

基于初始姿态估计的AMCL定位样本结果

AMCL是一种概率算法，计算机上的模拟结果可能与这里显示的示例运行略有不同。

在第一次AMCL更新后，通过采样平均值等于的高斯分布生成粒子amcl。InitialPose协方差等于初始协方差．

在8次更新后，粒子开始以更高的可能性收敛到区域:

在60次更新之后，所有粒子都应该收敛到正确的机器人姿态，激光扫描应该与地图轮廓紧密对齐。

为全局本地化配置AMCL对象。

在没有初始机器人姿态估计的情况下，AMCL将尝试在不知道机器人初始位置的情况下对机器人进行定位。该算法最初假设机器人在办公室自由空间的任何位置都有相同的概率，并在该空间内生成均匀分布的粒子。因此，与初始姿态估计定位相比，全局定位需要更多的粒子。

要启用AMCL全局本地化特性，请替换配置AMCL目标进行初始姿态估计定位使用本节中的代码。

amcl.GlobalLocalization=true；amcl.ParticleLimits=[500 50000]；

AMCL全球本地化的样本结果

AMCL是一种概率算法，计算机上的模拟结果可能与这里显示的示例运行略有不同。

首次AMCL更新后，粒子均匀分布在自由办公空间内：

在8次更新后，粒子开始以更高的可能性收敛到区域:

在60次更新之后，所有粒子都应该收敛到正确的机器人姿态，激光扫描应该与地图轮廓紧密对齐。

工具书类

[1] S.Thrun，W.Burgard和D.Fox，《概率机器人学》。麻省剑桥：麻省理工学院出版社，2005年。