连接到Gazebo的TurtleBot

首先，在下面的步骤产卵在虚拟机的办公环境内的模拟TurtleBot开始使用凉亭和模拟乌龟机器人(ROS工具箱)启动凉亭办公室世界从桌面，如下所示。

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

ipaddress =“192.168.2.150”;rosinit（IPADDRESS，11311）;

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

模拟办公环境的布局：

加载模拟世界的地图

在凉亭的办公环境中加载二元占用网格。地图是通过在办公环境内驾驶TurtleBot生成的。该地图使用Kinect®的距离方位读数和地面真实姿势露台/ model_states话题。

加载officemap.mat显示(图)

设置激光传感器型号和TurtleBot运动模型

TurtleBot可以被建模为差分驱动机器人及其运动可使用测距数据来估计。这噪音属性定义了机器人的旋转和直线运动的不确定性。增加了odometryModel。噪音当使用里程计测量传播粒子时，属性将允许更多的传播。指odometryMotionModel对财产的细节。

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

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

rangeFinderModel = likelihoodFieldSensorModel;rangeFinderModel。SensorLimits= [0.45 8]; rangeFinderModel.Map = map;

集rangeFinderModel。SensorPose到固定摄像机相对于机器人基座的坐标变换。这是用来转换激光读数从相机框架到TurtleBot的基础框架。请参考访问ROS的TF转型树(ROS工具箱)关于坐标变换的细节。

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

％查询转型树（TF树）中ROS。tftree = rostf;waitForTransform (tftree' / base_link '，'/ base_scan'）;sensorTransform = getTransform (tftree,' / base_link '，'/ base_scan'）;％获取欧拉旋转角度。laserQuat = [sensorTransform.Transform.Rotation.W sensorTransform.Transform.Rotation.X．．.sensorTransform.Transform.Rotation.Y sensorTransform.Transform.Rotation.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)];

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

从TurtleBot检索传感器和里程计测量创建ROS用户。

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

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

[velPub, velMsg] =．．.rospublisher ('/ cmd_vel'，'geometry_msgs /扭转'）;

初始化AMCL对象

实例化一个对象AMCLamcl．看到monteCarloLocalization以获取更多关于课程的信息。

amcl = monteCarloLocalization;amcl。UseLidarScan = true;

分配MotionModel和SensorModel在属性amcl对象。

amcl。MotionModel = odometryModel;amcl。SensorModel = rangeFinderModel;

当机器人的运动超过所述颗粒过滤器仅更新颗粒UpdateThresholds，定义[x, y，偏航]中的最小位移来触发滤波器更新。这可以防止由于传感器噪音而过于频繁的更新。粒子重采样发生在amcl。ResamplingInterval过滤器更新。在较慢的粒子收敛的价格使用较大的数字导致较慢的颗粒消耗为好。

amcl.UpdateThresholds = [0.2,0.2,0.2];amcl.ResamplingInterval = 1;

配置AMCL对象进行本地化与初始姿势估计。

amcl。ParticleLimits定义在重采样过程中产生的粒子数量的上限和下限。允许生成更多的粒子可能会提高收敛到真正机器人姿态的机会，但会影响计算速度，粒子可能需要更长的时间，甚至无法收敛。请参阅[1]中“KL-D抽样”一节计算粒子数的合理界值。注意，与初始姿态估计定位相比，全局定位可能需要更多的粒子。如果机器人知道自己的初始姿态有一定的不确定性，这样的附加信息可以帮助AMCL用较少的粒子数更快地定位机器人，即可以使用较小的上界值amcl。ParticleLimits．

现在设置amcl.GlobalLocalization为AMCL提供估计的初始姿态。通过这样做，AMCL的初始信念是机器人的真位姿服从平均值为的高斯分布amcl。InitialPose和协方差矩阵等于amcl。InitialCovariance．初始姿势估计应该根据您的设置来获得。这个例子帮助检索从凉亭机器人的当前真正的姿势。

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

amcl。ParticleLimits = [500 5000];amcl.GlobalLocalization= false; amcl.InitialPose = ExampleHelperAMCLGazeboTruePose; amcl.InitialCovariance = eye(3)*0.5;

安装助手的可视化和驾驶TurtleBot。

设置ExampleHelperAMCLVisualization来绘制地图并更新机器人在地图上的估计姿态、粒子和激光扫描读数。

visualizationHelper = ExampleHelperAMCLVisualization（地图）;

机器人运动的AMCL算法至关重要。在这个例子中，我们驾驶TurtleBot随机使用ExampleHelperAMCLWanderer类，同时使用避开障碍物的驱动该环境内部的机器人controllerVFH班级。

wanderHelper =．．.ExampleHelperAMCLWanderer（laserSub，sensorTransform，velPub，velMsg）;

本地化过程

所述AMCL算法与在当机器人周围移动每个时间步长测距和传感器读数更新。请允许几秒钟颗粒被初始化，并在图中画出了。在这个例子中，我们将运行numUpdatesAMCL更新。如果机器人不收敛到正确的机器人的位姿，考虑使用较大numUpdates．

numUpdates = 60;我= 0;而我接收激光扫描和里程计信息。scanMsg =接收(laserSub);odompose = odomSub.LatestMessage;％创建lidarScan对象传递给AMCL对象。扫描= lidarScan (scanMsg);％对于那些倒置安装的传感器，你需要扭转使用“倒装”功能扫描角读数％顺序。从测程信息中计算机器人的姿态[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）];使用新的里程计％更新估计机器人的姿势和协方差和％传感器读数。[isUpdated,estimatedPose, estimatedCovariance] = amcl(姿势，扫描);％驱动机器人下一姿态。漂移（wanderHelper）;在地图上绘制机器人的估计姿态、粒子和激光扫描图。如果isUpdated i = i + 1; / /更新plotStep(visualizationHelper, amcl, estimatedPose, scan, i)结尾结尾

停在MATLAB的TurtleBot和关机ROS

停止(wanderHelper);rosshutdown

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

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

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

第一次AMCL更新后，粒子由采样高斯分布生成，均值为amcl。InitialPose协方差等于amcl。InitialCovariance．

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

经过60次更新，所有的粒子应该收敛到正确的机械手姿势和激光扫描应与地图轮廓紧密接轨。

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

如果没有最初的机器人姿态估计可用，AMCL会尽量不知道机器人的初始位置来定位机器人。该算法最初假定机器人具有相等概率在办公室的自由空间是任意位置，例如空间内产生均匀分布的颗粒。因此，全球定位相比，与初始姿势估计本地化需要显著更多的粒子。

为了使AMCL全球定位功能，更换代码段中配置AMCL对象与初始姿势估计本地化使用本节中的代码。

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

AMCL全球本地化的样本结果

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

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

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

经过60次更新，所有的粒子应该收敛到正确的机械手姿势和激光扫描应与地图轮廓紧密接轨。

参考文献

S. Thrun, W. Burgard和D. Fox，概率机器人。麻省理工学院出版社，2005年。