ROS与Matlab语言入门教程-目标跟踪

在本例程中，用户将结合Kinect相机探索自主行为，该算法包含TurtleBot寻找蓝色球体和保持在球体的固定距离上。算法的实现依赖“ExampleHelperTurtleBotCommunicator”类，该类帮助与TurtleBot的通信。用户可以嵌入安全特性，如碰撞和悬崖传感器。

运行该例程需要图像处理工具箱。

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

连接到TurtleBot

确保用户拥有TurtleBot，运行在Gazebo仿真中或者真实硬件都可以。有关启动步骤参考4.2开始使用Gazebo和仿真的TurtleBot或4.1开始使用真实TurtleBot。如果用户使用硬件，那么需要寻找一个蓝色球体用于跟踪。如果使用Gazebo，蓝色球体必须在环境中，位于机器人的前方（确保用户使用“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上的终端执行该指令。

从“ExampleHelperTurtleBotCommunicator”类创建TurtleBot对象，使能相机和声音。如果用户使用Gazebo，声音话题并不存在，因此TurtleBot通信器将创建一个（虽然没有发布声音）。

tbotAdmin = ExampleHelperTurtleBotCommunicator();
enableCamera(tbotAdmin);
enableSound(tbotAdmin);
handles.Tbot = tbotAdmin;

调整蓝色球体检测

为图像滤波器设置参数，增加这些参数到结构中，该结构将在算法中应用。

blueBallParams.blueMax = 120; % Maximum permissible deviation from pure blue
blueBallParams.darkMin = 30; % Minimum acceptable darkness value

尝试可视化球体，确保寻找球体的参数能够定位球体。运行“exampleHelperTurtleBotFindBlueBall”函数查看是否存在一个球体。如果存在，则“c”和“m”已经赋值。“ball”是一个二进制图像，通过在图像上应用蓝色和黑色滤波器创建。查看“ball”检查蓝色球体被正确隔离。

[c,~,ball] = exampleHelperTurtleBotFindBlueBall(tbotAdmin.ImColor,blueBallParams);
exampleHelperTurtleBotPlotObject(tbotAdmin.ImColor,ball,c);

上面的代码将显示图像并在球体的中心绘制共色加号。如果没有找到球体，尝试增加或者减少“blueBallParams.blueMax”和“blueBallParams.darkMin”。再次查看图像直到找到球体。这是一种好的办法在使用控制器之前很好地调整寻找球的算法。当用户在Gazebo中运行这行命令，将看到类似如下的图（左侧是Gazebo示例图，右侧是实物图）。

与Gazebo仿真环境中调整颜色阈值相比，真实环境中颜色阈值的调整显得很有挑战性。
在用户很好的调整好参数之后，增加参数到“handles”对象中，该对象应用于跟踪球体算法。

handles.params = blueBallParams;

测试固定距离控制器

为TurtleBot设置控制器增益，TurtleBot使用PID控制器以保持和球体的固定距离。

下面的设置代码中，第一个对于Gazebo中的TurtleBot是好的增益，第二个是真实硬件的TurtleBot的好的增益。调整增益当用户看到良好的效果。

Effective gains for Gazebo simulation
gains.lin.pgain = 1/100; % Assign pgain value in the struct
% Here is a more compact way to assign the struct values.
gains.lin = struct('pgain',1/100,'dgain',1/100,'igain',0,'maxwindup',0','setpoint',0.65);
gains.ang = struct('pgain',1/400,'dgain',1/500,'igain',0,'maxwindup',0','setpoint',0.5);
% THESE SHOULD BE USED IN GAZEBO
Effective gains for TurtleBot hardware
gains.lin = struct('pgain',1/100,'dgain',1/1000,'igain',0,'maxwindup',0','setpoint',0.75);
gains.ang = struct('pgain',1/100,'dgain',1/3000,'igain',0,'maxwindup',0','setpoint',0.5);
% THESE SHOULD BE USED ON HARDWARE

确保增加“gains”结构到“handles”变量中。

handles.gains = gains;

定义定时器通过回调函数执行跟踪行为。定义停止函数以停止ROS。包含定时器回调函数中的句柄。

timer2 = timer('TimerFcn',{@exampleHelperTurtleBotTrackingTimer,handles},'Period',0.1,'ExecutionMode','fixedSpacing');
timer2.StopFcn = {@exampleHelperTurtleBotStopCallback};

使用如下的命令启动定时器，用户可以看到TurtleBot开始在环境中移动，寻找球体。当在Kinect图像中发现球体，机器人将使用控制器保持在固定的距离。

start(timer2);

在仿真中没有激活碰撞传感器，所以TurtleBot可能在撞墙的时候不能恢复。

如果用户想要在环境中移动蓝色球体，使用如下的命令应用力到球体。

g = ExampleHelperGazeboCommunicator();
ballhandle = ExampleHelperGazeboSpawnedModel('unit_sphere_1',g)
duration = 2;
forceVector = [0 4 0];
applyForce(ballhandle,'link',duration,forceVector)

如果用户想要进一步探索仿真中Gazebo控制，参考5.1从Gazebo读取模型和仿真性能和5.2在Gazebo中增加、建立和移除对象。

停止机器人运动

使用如下的命令停止定时器和自主行为。

stop(timer2);

如果定时器在停止之前被从工作空间清除，用户需要用另一种方法删除。执行下述命令停止所有定时器（即使定时器在后台运行）。

delete(timerfindall)

当用户完成工作后，清楚工作空间中的发布器、订阅器和其它有关ROS的对象是个良好的习惯。

clear

更多信息

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

本例中，支持文件的组织允许用户在定制和再利用代码具有很高的灵活性。用户可以通过更改“handles”结构中的值，更改寻找球体的参数和控制器的增益。本例程应用一个定时器处理控制算法的各个方面，该定时器是“exampleHelperTurtleBotTrackingTimer”。这个定时器有名称数值对参数“Period”和“ExecutionMode”，用于决定多长时间调用一次定时器的回调函数。此外，还应用了停止调用函数。用户可以根据自己的需要增加回调函数。

传递到定时器的句柄包括球体寻找的“params”、控制器的“gains”和代表TurtleBot类的“Tbot”。TurtleBot通信器类（ExampleHelperTurtleBotCommunicator）通过方便的函数和数据存储，使得TurtleBot的通信和控制变得很容易。使用该类，用户可以探索TurtleBot的许多方面，以及结合该类写自己的函数。一些有用的函数如下所示：

Tbot = ExampleHelperTurtleBotCommunicator();
setVelocity(Tbot, linearVelocity, angularVelocity)
enableCamera(Tbot) % and disableCamera(Tbot)
enableLaser(Tbot) % and disableLaser(Tbot)
enableOdom(Tbot) % and disableOdom(Tbot)

使能一些订阅器之后，用户可以通过如下方法获取数据：

colorImage = Tbot.ImColor;
laserScanData = Tbot.LaserData;
odometry = Tbot.Pose;
bumperArray = Tbot.BumpArray;
cliffFlag = Tbot.CliffFlag;

用户可以探索该类包含其它函数。

在本例中，用户使用相机和声音性能，还有速度发布器和碰撞/悬崖传感器。所有这些性能都是通过“Communicator”类控制和管理的。

“exampleHelperTurtleBotTrackingTimer”的结构是简单的，这是一个需要以下初始化步骤的基本的状态机。初始化函数决定当没有在悬崖或碰撞恢复状态中选择哪个跟踪算法和控制器。该函数是：

function [objectTrack, imgControl] = initControl()
% INITCONTROL - Initialization function to determine which control
% and object detection algorithms to use
objectTrack = @exampleHelperTurtleBotFindBlueBall;
imgControl = @exampleHelperTurtleBotPointController;

在本例中，跟踪函数是“exampleHelperTurtleBotFindBlueBall”，控制器是“exampleHelperTurtleBotPointController”。用户可以通过自定义函数替换该函数，只要具有相同的输入和输出参数结构。“forexampleHelperTurtleBotFindBlueBall”的输入参数是彩色图像和寻找球体参数结构。输出参数是中心、大小和寻找对象的二进制图像。“exampleHelperTurtleBotPointController”的输入参数是中心、大小（虽然本例中没有应用）、图像存储和控制器增益（一个结构）。输出参数是线性和角度速度。“exampleHelperTurtleBotTrackingTimer”使用的基本状态机是：

switch state
case ExampleHelperTurtleBotStates.Seek
% Object-finding state
[center, scale] = findObject(handles.Tbot.ImColor,handles.params);
% Wander if no circle is found, target the circle if it exists
if isempty(center)
[linearV, angularV] = exampleHelperTurtleBotWanderController();
else
[linearV, angularV] = imageControl(center, scale, size(handles.Tbot.ImColor),handles.gains);
setSound(handles.Tbot,2);
end
state = ExampleHelperTurtleBotStates.Seek;
case ExampleHelperTurtleBotStates.Bumper
% Bumper contact state
case ExampleHelperTurtleBotStates.Spin
% Spin state
case ExampleHelperTurtleBotStates.Cliff
% Cliff avoidance
end

用户可以增加或者减少状态机的状态，如果用户想要更改状态的名称，使用“ExampleHelperTurtleBotStates”类。

寻找球体算法是模块化且可更改的，该算法使用了两个图像滤波器（一个是黑色一个是蓝色）掩膜在一起以分离出蓝色球体。用户可以更改掩膜以寻找红色或者绿色的球体。如果用户想要探索其它图形跟踪模式，基本的工作流程是一样的。

蓝色通道被分离出来（带有一定程度的缩放因子）且一个阈值应用于产生二进制图像掩膜。

blueImg = img(:,:,1)/2 + img(:,:,2)/2 - img(:,:,3)/2;
blueThresh = blueImg < params.blueMax;

这些代码分离出蓝色的逆（带有不同的缩放）并强调了黑色，应用了一个阈值。

darkIso = -img(:,:,1)/2 - img(:,:,2)/2 + 3img(:,:,3) - 2rgb2gray(img);
darkThresh = darkIso > params.darkMin;

将两个二进制图像掩膜在一起，以分离出深蓝色球体。

ball1 = blueThresh & darkThresh;

图像上的常量和缩放因子由用户决定以分立特定的颜色，用户可以采用不同的组合进行实验。

用户还可以使用“regionprops”寻找滤波后的图像的连续区域，“regionprops”是图像处理工具箱的一部分。

s = regionprops(ball1, {'Centroid','Area','EquivDiameter'});

该区域寻找球体还有很多其它步骤，用户可以在“exampleHelperTurtleBotFindBlueBall”中找到。“exampleHelperTurtleBotPointController”函数使用“ExampleHelperPIDControl”类，保持图像中的准确的位置的一个特定点（此处是球体的中心位置）。

本例中代码的模块化和灵活性允许用户实验自己的算法和函数。