连接Gazebo的TurtleBot

首先，按照中的步骤在虚拟机的办公环境中生成一个模拟TurtleBot开始使用露台和模拟的高跷机器人（ROS工具箱）启动露台办公室世界从桌面，如下所示。

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

IP地址='192.168.2.150'；罗西尼特（IP地址，11311）；

使用NodeURI初始化全局节点/matlab_全局_节点_73841http://192.168.2.1:53461/

模拟办公环境的布局：

加载模拟世界的地图

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

负载办公地图显示(图)

建立激光传感器模型和TurtleBot运动模型

TurtleBot可以被建模为一个差动驱动机器人，它的运动可以通过里程测量数据来估计噪音属性定义了机器人旋转和直线运动的不确定性里程表模型。噪音属性将允许在使用里程计测量传播粒子时进行更多传播。请参阅里程计模型对财产的细节。

odometryModel=odometryModel；odometryModel.Noise=[0.20.20.20.2]；

TurtleBot上的传感器是从Kinect读数转换而来的模拟测距仪。通过将测距仪测量的终点与占用地图进行比较，使用似然场方法计算感知一组测量的概率。如果终点与占用地图中的占用点相匹配，则pe的概率R进行此类测量的可能性很高。应调整传感器模型，使其与实际传感器特性相匹配，以获得更好的测试结果。该特性传感器极限定义传感器读数的最小和最大范围。财产地图定义用于计算可能性字段的占用地图。请参阅likelihoodFieldSensorModel对财产的细节。

rangeFinderModel = likelihoodFieldSensorModel;rangeFinderModel。传感器极限= [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];laserRotation = quat2eul (laserQuat,“ZYX”);%设置| SensorPose |，其中包括沿基本链接的平移%+X、+Y方向（以米为单位）以及沿基准链接的+Z轴的旋转角度%的弧度。rangeFinderModel.SensorPose=．..[sensorTransform.Transform.Translation。X sensorTransform.Transform.Translation.Y laserRotation (1)];

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

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

laserSub=rossubscriber(“/扫描”)；odomSub=rossubscriber(“/奥多姆”);

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

[velPub，velMsg]=．..罗斯出版社(“/cmd_vel”，“几何图形\u msgs/Twist”);

初始化AMCL对象

实例化AMCL对象amcl.看monteCarloLocalization以获取更多关于课程的信息。

amcl=蒙特卡洛定域化；amcl.UseLidarScan=真；

分配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提供估计的初始姿态。通过这样做，AMCL的初始信念是机器人的真位姿服从平均值为的高斯分布初始姿势协方差矩阵等于amcl。InitialCovariance. 应根据您的设置获得初始姿势估计值。此示例帮助器从Gazebo检索机器人的当前真实姿势。

请参阅第节为全局本地化配置AMCL对象有关使用全局本地化的示例。

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

用于可视化和驱动TurtleBot的设置帮助程序。

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

visualizationHelper=ExampleHelperAMCLVisualization（地图）；

机器人运动对于AMCL算法至关重要。在本例中，我们使用ExampleHelperCamcLwander类随机驱动TurtleBot，该类在环境中驱动机器人，同时使用控制器FH班级。

流浪助手=．..例如HelperCamcLwander（laserSub、sensorTransform、velPub、velMsg）；

本地化程序

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

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

停止TurtleBot并在MATLAB中关闭ROS

停止(wanderHelper);rosshutdown

使用NodeURI关闭全局节点/matlab\u全局节点\u 73841http://192.168.2.1:53461/

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

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

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

更新8次后，粒子开始会聚到可能性更高的区域：

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

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

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

要启用AMCL全局本地化功能，请替换中的代码部分使用初始姿势估计配置用于定位的AMCL对象使用本节中的代码。

全球本地化= true; amcl.ParticleLimits = [500 50000];

AMCL全球本地化的示例结果

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

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

更新8次后，粒子开始会聚到可能性更高的区域：

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

工具书类

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