连接到TurtleBot露台

首先,产生一个模拟TurtleBot内部办公环境中的虚拟机按照步骤从露台和模拟TurtleBot开始(ROS工具箱)启动露台的办公室世界从桌面,如下所示。

在MATLAB在主机实例,运行以下命令初始化ROS全球节点在MATLAB和连接到ROS主虚拟机通过其IP地址ipaddress。取代ipaddress与你TurtleBot虚拟机的IP地址。

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

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

模拟办公环境的布局:

加载的模拟世界地图

加载一个二进制占用网格露台的办公环境。地图是由驾驶TurtleBot在办公环境。地图使用Kinect的距离方位读数构造®和地面真理构成露台/ model_states的话题。

负载officemap.mat显示(图)

安装激光传感器模型和TurtleBot运动模型

TurtleBot可以建模为一个差动驱动机器人和它的运动可以使用测程法估计数据。的噪音属性定义了机器人的旋转和直线运动的不确定性。增加了odometryModel.Noise财产将允许更多的传播传播粒子使用测程法测量。指odometryMotionModel对财产的细节。

odometryModel = odometryMotionModel;odometryModel.Noise= [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只有兼容传感器固定在机器人的框架,这意味着传感器变换是恒定的。

%查询转换树在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…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)];

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

创建ROS用户获取传感器和测程法从TurtleBot测量。

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

创建发送速度命令TurtleBot ROS的出版商。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定义了低和上限的粒子数量将在重采样过程中生成。允许更多的粒子生成可能改善收敛于真实的机器人位姿的机会,但影响计算速度和粒子可能需要更长时间,甚至无法收敛。请参阅“KL-D抽样”部分[1]计算一个合理的界值的粒子数。注意全球本地化可能需要更多的粒子相比,定位与初始姿态估计。如果机器人知道最初带来一些不确定性,这些额外的信息可以帮助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;而我< numUpdates%接收激光扫描和量距消息。scanMsg =接收(laserSub);odompose = odomSub.LatestMessage;%通过AMCL对象创建lidarScan对象。扫描= 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我= + 1;plotStep (visualizationHelper amcl estimatedPose,扫描,我)结束结束

在MATLAB停止TurtleBot和关闭ROS

停止(wanderHelper);rosshutdown

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

样本结果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后更新,所有的粒子都应该收敛到正确的机器人位姿和激光扫描应密切结合地图轮廓。

引用

[1]杜伦,w . Burgard和d·福克斯,概率机器人。剑桥,麻州:麻省理工学院出版社,2005年。