在凉亭连接到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

模拟办公环境布局:

加载模拟世界的地图

在凉亭中加载一个办公环境的二元占用网格。该地图是通过在办公环境中驾驶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 = likehoodfieldsensormodel;rangeFinderModel。SensorLimits= [0.45 8]; rangeFinderModel.Map = map;

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

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

查询ROS中的转换树(tf树)。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];laserQuat = quat2eul(激光quat，“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(' /奥多姆'）;

创建ROS发布器，用于向TurtleBot发送速度命令。TurtleBot订阅“/ mobile_base /命令/速度”对于速度命令。

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

初始化AMCL对象

实例化一个AMCL对象amcl．看到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 Sampling”一节计算合理的粒子数限值。请注意，与初始姿态估计的定位相比，全局定位可能需要更多的粒子。如果机器人知道它的初始姿态具有一定的不确定性，这些额外的信息可以帮助AMCL用更少的粒子数更快地定位机器人，即您可以使用更小的上界值amcl。ParticleLimits．

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

详情请参阅配置全局本地化的AMCL对象这是一个使用全局本地化的例子。

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

设置帮助可视化和驾驶TurtleBot。

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

visualizationHelper = ExampleHelperAMCLVisualization(map);

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

wanderHelper =.．.helperamclwanderer (laserSub, sensorTransform, velPub, velMsg);

本地化过程

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

numUpdates = 60;I = 0;而i < numUpdates接收激光扫描和测程信息。scanMsg =接收(laserSub);odompose = odomSub.LatestMessage;创建lidarScan对象传递给AMCL对象。扫描= lidarScan(scanMsg);%对于倒置安装的传感器，需要将%顺序的扫描角度读数使用'翻转'功能。从测程信息中计算机器人的姿态[x,y，偏航]。odomQuat = [odompose. pose. orientation]。W odompose.Pose.Pose.Orientation.X.．.odompose.Pose.Pose.Orientation。Y, odompose.Pose.Pose.Orientation.Z];odomRotation = quat2eul(odomQuat);pose = [odompose.Pose.Pose.Position.]X, odompose. pose. position . y odomRotation(1)];%更新估计机器人的姿态和协方差使用新的里程计和%传感器读数。[isUpdated,estimatedPose, estimatedco方差]= 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。粒子限制= [500 50000];

AMCL全局本地化的样本结果

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

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

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

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

参考文献

[1] S. Thrun, W. Burgard和D. Fox，概率机器人。马萨诸塞州剑桥:麻省理工学院出版社，2005年。