ROS与Matlab语言入门教程-使用TurtleBot进行壁障

本例程演示使用TurtleBot实现VFH+壁障算法，使用定时器显示了在自主算法的性能。

VFH+算法是一个简单、局部的方法帮助机器人不碰撞障碍物的在空间中导航。因为算法只包含布局信息，所以不能保证机器人抵达目标点。在角落（局部最小）可能被卡主。如果目标瞄准增益相对壁障多大，机器人可能撞上墙壁，如果瞄准壁障增益相对壁障过小，则机器人可能无目的的移动。实验算法的不同增益额参数研究机器人行为的不同类型。

预备知识：4.3TurtleBot的通信，4.4探索TurtleBot的基本行为，4.5使用操纵杆控制TurtleBot。

连接到TurtleBot

确保用户有一个运行的TurtleBot，通过Gazebo仿真的机器人或者真实机器人都可以。启动步骤参考4.2开始使用Gazebo和仿真的TurtleBot。对于本例程，使用Gazebo的“TurtleBot World”将提供最好的环境。

初始化ROS。通过TurtleBot的IP地址替换示例IP地址（192.168.1.1）并连接到TurtleBot。

ipaddress = '192.168.1.1'
rosinit(ipaddress)

如果用户使用实际的TurtleBot硬件，确保启动了Kinect相机，命令是“roslaunch turtlebot_bringup 3dsensor.launch”，在TurtleBot上的终端执行该指令。

初始化壁障算法

生成一个结构包含在VFH+算法中使用的增益，为了更改机器人的行为，在初始化定时器之前更改这些增益。这些增益控制四个行为：瞄准目标，按直线运动，沿着连续的路径移动，避免撞向障碍物。不同的增益导致不同的行为。例如，如果obstaclesAvoid = 0，机器人则尝试冲过障碍物而忽略距离。如果goalTargeting = 0，则机器人在壁障的同时无目的的漫游。本例程提供更多的信息帮助用户选择合适的参数。这些增益公正的平衡瞄准目标和壁障的任务。

gains.goalTargeting = 100; % Gain for desire to reach goal
gains.forwardPath = 0; % Gain for moving forward
gains.continuousPath = 0; % Gain for maintaining continuous path
gains.obstacleAvoid = 5; % Gain for avoiding obstacles

注意，Kinect激光扫描具有最小距离限制。由于该最小距离限制，TurtleBot可以很好的避开一些障碍并转弯，当障碍非常接近时则会撞上。这种行为是因为激光看不到他们。这种运动在室内环境经常发生，在转向目标位置之前，TurtleBot还未完全达到阈值。在这一点上室内环境可能太近以致不能检测到，TurtleBot驱动到此处却并未意识到。这是结合局部路径规划算法结合激光扫描仪距离限制的缺点。对于真实的硬件，这种情况下需要激活碰撞传感器，但是在仿真中碰撞传感器不能工作，因此机器人可能被卡在门的结构中。

创建发布器和订阅器并让他们作为结构（timerHandles）的一部分，该结构创建的时候被放进定时器中。发布器用于速度，订阅器用于激光扫描仪、里程计和碰撞传感器。

timerHandles.pub = rospublisher('/mobile_base/commands/velocity'); % Set up publisher
timerHandles.pubmsg = rosmessage(rostype.geometry_msgs_Twist);
timerHandles.sublaser = rossubscriber('/scan'); % Set up subscribers
timerHandles.subodom = rossubscriber('/odom');
timerHandles.subbump =     rossubscriber('/mobile_base/sensors/bumper_pointcloud');

如果用户想要在处理之前重置里程计，用户必须订阅“reset_odometry”话题并发送一个空消息到该话题。

odomresetpub = rospublisher('/mobile_base/commands/reset_odometry'); % Reset odometry
odomresetmsg = rosmessage(rostype.std_msgs_Empty);
send(odomresetpub,odomresetmsg)

增加增益到“timerHandles”结构：

timerHandles.gains = gains;

测试壁障

初始化定时器。定时器函数取一系列名称和值的成对参数。第一对参数是定时器的回调函数，还包含了之前定义的结构。第二对参数是定时器的周期（此处每个循环0.1s）。第三和也是最后一对参数对是执行的模式，此处是固定的空间。用户还可以定义定时器的停止函数，此处当定时器停止的时候关闭ROS。

timer1 = timer('TimerFcn',{@exampleHelperTurtleBotObstacleTimer,timerHandles},'Period',0.1,'ExecutionMode','fixedSpacing');

在开启定时器之前，用户可以在命令行可视化算法的一些步骤。用户可以使用“theexampleHelperTurtleBotShowGrid”看到VFH+算法工作的基本方式，显示了三个绘图。图1显示了被存储为2D直方图的原始激光数据，图2显示了被平滑后的直方图以计算机器人的宽度，图3显示角度直方图，该直方图是由根据机器人能够行进的方向将障碍物分成几类创建的。VFH+算法使用这些步骤决定如何在瞄准目标点的同时壁障。

exampleHelperTurtleBotShowGrid(timerHandles.sublaser)

在Gazebo仿真中，直方图开起来类似下图所示。

“Angle bin”图从两个x-y网格图转换可视化得到。该网格表示真实的2D平面和实际X和Y轴（虽然坐标并不以真实世界一致，因为它们代表bins）。在角度图里，这些角度分组集合从左到右增加的规则列出，但是在2D平安这些角度与空间一致，从右到左，是相反的。

运行机器人

输入如下的命令启动定时器，TurtleBot将开始移动并避开所有检测到的障碍物。

start(timer1);

为了在循环的中间停止定时器，关闭绘图窗口。如果定时器没有清除，清除所有定时器的方法如下代码所示。

delete(timerfindall)

启动的时候，Gazebo仿真和环境绘图如下所示。

在选择旁边的点作为目标后，用户可以看到类似如下的图。

更多信息

注意：本小节的代码并不是在MATLAB命令行中执行的。

在本例中的代码可以更改以允许用户能够更自由的探索TurtleBot。如下介绍描述例程的特性和有关修改和使用的可选方案的建议。

这个脚本使用定时器，它可以应用于许多不同的方法。本例子中用户将会使用“TimerFcn”和“StopFcn”回调函数。

定时器的回调函数和基本的工作空间通过“timerHandles”结构共享信息，该结构包含了增益、发布器和订阅器。如果用户需要从基本工作空间增加信息到定时器回调函数，增加到“timerHandles”结构中。

一旦启动了定时器，定时器回调函数将根据参数运行。在本例程中，定时器回调函数（exampleHelperTurtleBotObstacleTimer）调用两个基本的函数执行算法的主要功能。此处是基本的结构（不包含声明、初始化和回调函数）。

function exampleHelperTurtleBotObstacleTimer(mTimer, event, handles)
% Declarations and Initializations would be here
% Determine current time
currentTime = datetime(event.Data.time);
% Execute the VFH+ algorithm to determine the desired angle trajectory
angleTarget = exampleHelperTurtleBotComputeTargetAngle(goal, data, pose, anglePrev, handles.gains);
% Execute the main control loop
[linV, angV] = turtlebotController(currentTime,angleTarget,bumper);
% Set the velocities
handles.pubmsg.Linear.X = linV;
handles.pubmsg.Angular.Z = angV;
send(handles.pub,handles.pubmsg);
% Set the previous angle
anglePrev = angleTarget;
% Callback functions would be here

第一个主要的函数是“exampleHelperTurtleBotComputeTargetAngle”，该函数返回一个角度，机器人将会转向瞄准该角度（相对于当前的方向）。在本例中，计算目标角度是VFH+算法实现的。计算“TargetAngle”的函数可以被任何用户定义的函数替换，只要能够返回目标角度。用户可以探索许多潜在的算法，VFH+算法的基本步骤参考Ulrich 和 Borenstein的文章1。算法的关键步骤如下图所示。

如果用户想要调整直方图的尺寸，激光扫描的最大最小值和角度直方图，参考“exampleHelperTurtleBotComputeTargetAngle”的叫做“SetBin”的本地函数。用户可以修改任何参数，包括x和y的最大最小值和步进尺寸。注意机器人的物理限制（例如，y的最小值不能够是负值因为机器人无法看到自己的背后）。

定时器回到函数中的第二个主要的方法是“turtlebotController”，当给定参数例如目标位置、当前位姿和目标角度的时候，该方法返回线性和角度速度。该控制器可以被任何用户定义的控制器替换，只要能够返回线性和角度速度（m/s）。“turtlebotController”函数中的类是“ExampleHelperPIDControl”。当前的实现，“turtlebotController”仅仅使用了比例控制器。然而，PID控制类具有正比，微分，和积分控制选项，可以用于如下的控制方式。

linGains.pgain = 0.2;
linGains.dgain = 0;
linGains.igain = 0;
linGains.maxwindup = 0;
linGains.setpoint = 0;
linPID = ExampleHelperPIDControl(linGains);

一个更快速的设置增益的方法：

linGains = struct('pgain',0.2,'dgain',0,'igain',0,'maxwindup',0','setpoint',0);

务必测试和调整所选择的增益，因为不同的增益可能导致非常不同的机器人行为。“Setpoint”是用户想要控制的点，结构的其它元素已经明确命名。为更新控制器和返回控制的值，使用更新函数。

controlvalue = update(linPID,currentpoint);

回调函数订阅器在定时器回调函数中被定义为嵌套的函数。重要的变量如“pose”、“goal”和“bumper”在这些回调函数中定义。所有的定时器回调函数和支持的函数的模块性都允许用户化具有很大的灵活性。