ros自己写避障算法_基于ROS的机器人室内环境探索、避障和目标跟踪方法与流程...

柔光的暖阳◎ 2023-01-09 01:16 288阅读 0赞

本发明涉及机器人运动的技术领域，尤其涉及一种基于ROS的机器人室内环境探索、避障和目标跟踪方法。

背景技术：

随着科学技术的发展及社会的进步，移动机器人技术得到了迅速的发展。目前，机器人的应用已经涉及卫生、医疗、导游、教育、娱乐、安保、日常生活等多个领域，适用于各种工作环境，甚至危险、肮脏和枯燥的场合等。在很多情况下，作业空间的信息是未知的，机器人进入到未知环境时，需要对作业环境进行有效地探测，构建出作业环境的地图。只有在构建的地图的基础上才能进行导航、路径规划、避障策略和其他操作。在未知环境中进行环境探索，并根据激光雷达获取的信息构建相应的环境地图是移动机器人需要必备的基本能力。然而在很多环境中机器人事先无法获取工作环境的地图，例如在矿井探测、深海探索、危险环境救援等工作环境下，通常人类无法进入现场获取环境信息，必须依赖移动机器人进行环境的探测和模型的创建。当机器人在对目标进行跟踪时，由于机器人的运动、摄像机的抖动、跟踪目标无规律的运动、光照的影响等因素，都会增加机器人跟踪目标的复杂性。移动机器人通过操作系统、运动控制、路径规划、避障、跟踪、环境建图与定位等，完成各种各样的任务。

随着机器人领域的快速发展和复杂化，对代码复用和模块化的需求日益强烈。ROS是运行在Ubuntu等主操作系统上的次级操作系统，具有分布式的开源软件架构，将机器人的底层硬件进行抽象，提高代码的复用率，具备底层驱动程序管理和共用功能的执行，能够提供类似传统操作系统的诸多功能，包括常用功能实现、进程间消息传递和程序包管理等，此外，还提供了相关的工具和库，用于获取、编译、编辑代码以及在多个计算机之间运行程序，完成分布式计算。ROS能够支持多种机器人构型和传感器，使研究人员快速的进行开发和仿真。

ROS系统支持C++、Python等多种编程语言，集成了用于机器人视觉开发的OpenCV库，拥有SLAM地图构建和导航功能包。ROS可以使用标准化机器人描述格式(URDF)建立自己的机器人模型，也可以使用Gazebo仿真软件，建立理想的模拟环境，在模拟环境中，驱动机器人进行避障，路径规划，地图构建和导航等仿真实验。

目标跟踪方法主要分为基于区域的跟踪、基于特征的跟踪、基于模型的跟踪和基于主动轮廓的跟踪。大多数基于视觉传感器的目标跟踪，都是基于颜色特征的目标跟踪，如Comaniciu等人在文献中将MeanShift算法应用于目标跟踪，使MeanShift算法在目标跟踪领域得到广泛应用。移动机器人使用传统的MeanShift跟踪方法跟踪目标时，目标跟踪窗口不能自适应调整，无法反映目标的运动状况，当存在同色物体干扰、目标快速移动以及遮挡时，该方法的跟踪效果不太理想。Bradski在文献中提出了Camshift算法，使用颜色直方图，计算目标窗口的颜色概率分布图，实现目标跟踪。移动机器人使用Camshift方法跟踪目标时，当目标被完全遮挡或目标运动速度变化过快时，移动机器人会丢失目标。

技术实现要素：

针对现有移动机器人进行目标跟踪时，当目标被完全遮挡或目标运动速度变化过快时，移动机器人会丢失目标的技术问题，本发明提出一种基于ROS的机器人室内环境探索、避障和目标跟踪方法，基于ROS系统在移动机器人平台上使用激光雷达传感器，通过上位机的远程操控实现移动机器人的自主探索、避障功能，可以在无需用户干预的前提下，实现对未知室内环境的自主探索与地图构建，解决了移动机器人目标跟踪的实时性问题和完全遮挡问题。

为了达到上述目的，本发明的技术方案是这样实现的：一种基于ROS的机器人室内环境探索、避障和目标跟踪方法，其步骤如下：

步骤一：搭建ROS移动机器人的硬件平台：移动机器人的底部设置有运动控制模块，移动机器人的上部固定有传感器，移动机器人的中部设有控制器和无线通信模块，控制器上安装ROS系统，运动控制模块、传感器和无线通信模块均与控制器相连接，无线通信模块和上位机相连接

步骤二：将ROS移动机器人设置在待检测的室内，利用传感器中的激光雷达扫描室内环境，利用传感器中的里程计采集移动机器人的位置信息和方向信息，ROS移动机器人通过方波路径探索室内环境，上位机通过无线通信模块实施获取激光雷达的扫描信息，上位机中的ROS系统利用构建地图功能包构建栅格地图；

步骤三：将建立好的栅格地图导入ROS移动机器人的ROS系统中，使用构建的栅格地图导航机器人到地图内的指定位置，视觉传感器基于Kalman滤波方法与MeanShift跟踪方法实现目标的跟踪。

所述运动控制模块主要是Kobuki移动底盘，传感器包括里程计、Kinect2.0深度的视觉传感器和Rplidar A1激光雷达，控制器为安装了Ubuntu14.04和ROS indigo系统的Jetson TK1开发板，无线通信模块为Intel 7260AC HMW无线网卡，无线通信模块通过wifi模块实现数据的传输；控制器上配置了适用于ROS系统的Kinect2.0深度视觉传感器、Rplidar A1激光雷达和无线网卡Intel 7260AC HMW的驱动系统及Kobuki2.0深度视觉传感器的软件系统；上位机上搭建了Ubuntu14.04和ROS indigo操作系统，上位机通过SSH远程登录到控制器的Ubuntu系统，启动机器人的移动底盘，上位机通过无线wifi模块使用ROS系统的通信方式，发布速度消息给机器人移动底盘，改变机器人的线速和角速度，控制机器人运动。

所述方波路径探索的方法为：启动时，将ROS移动机器人放置到周围1米内没有障碍物的室内地方，ROS移动机器人开始向未知区域移动，激光雷达探测到前方室内出现墙壁后，当ROS移动机器人距离墙壁小于0.8米后，控制器控制运动控制模块开始进行转向，避开前方墙壁；转向过程中，激光雷达探测到前方没有障碍物时，ROS移动机器人开始停止旋转，重新开始向前移动，继续进行探索，此时控制器中的定时器被触发，开始计时，当移动10秒后，ROS移动机器人停止移动，按照避障时的旋转方向再旋转90度，旋转完成后继续进行移动，直到遇到墙壁时再旋转，此时的避障旋转方向和之前的旋转方向相反，重复之前步骤，直到完成室内环境探索。

在探索中ROS移动机器人使用人工势场避障方法进行避障，人工势场避障方法的步骤为：

(1)设置起始点位置为ps＝\[xs,ys\]T，目标点位置为pt＝\[xt,yt\]T；将ROS移动机器人看作一个质点，并且在二维空间中运动；

(2)上位机通过ROS系统的tf坐标变换求出ROS移动机器人在全局坐标系中的位置pc＝\[xc,yc\]T，利用激光雷达探测到的障碍物位置为po＝\[xo,yo\]T；

(3)计算ROS移动机器人所受虚拟势场的总斥力与引力的合力为：其中，U(pc)＝Ua(pc)+Ure(pc)为引力势和斥力势之和，为目标对ROS移动机器人形成的引力势，为障碍物对ROS移动机器人形成的斥力势，λ、k、d0均为常数，为机器人同目标之间的欧几里得距离，为机器人同障碍物之间的欧几里得距离；为由引力场产生的引力；

为障碍物对ROS移动机器人的斥力；当有多个障碍物时，计算各个障碍物对机器人产生的斥力，将多个障碍物产生的斥力合成一个总斥力；

(3)将合力的方向作为机器人的避障方向，ROS移动机器人旋转到避障方向进行运动，实现机器人的局部避障。

所述ROS移动机器人的激光雷达扫描室内环境信息，上位机通过wifi模块更新激光雷达实时返回的数据，上位机调用ROS系统的构建地图功能包，当激光雷达返回的数据中出现障碍物时，上位机中的rviz可视化工具将栅格地图使用黑色显示出探测的障碍物区域，使用浅灰色描述探测过的没有障碍物的区域，利用深灰色显示未探索的区域。

所述步骤三中导航的方法为：上位机使用rviz可视化工具，运行激光雷达的rplidar\_amcl.launch启动文件，使用map\_file或在.bashrc文件的TURTLEBOT\_MAP\_FILE环境变量中，将构建的栅格地图导入ROS移动机器人；在栅格地图内，机器人进行二维位姿估计时，设置机器人的初始位姿方向，机器人开始进行旋转，旋转到设定的方向后，停止旋转；指定机器人在实际环境中的方向，使用二维目标导航设定机器人的导航位姿；当设定导航目标后，机器开始规划路径，路径规划完成后，机器人开始沿着规划的路径朝目标移动，通过人工势场避障方法避开障碍物，机器人到达目标位置后，停止移动，并旋转到目标位姿方向后，停止旋转，到达栅格地图内的指定位置。

所述视觉传感器利用实时目标跟踪节点实现目标跟踪，其步骤为：

步骤(a)：对Kalman滤波的状态转移矩阵A、观测矩阵H、过程噪声协方差矩阵Q、测量噪声协方差矩阵R和状态误差协方差矩阵P参数进行初始化，建立Kalman跟踪对象参数；

步骤(b)：根据前一帧的目标状态位置，使用目标的跟踪状态：X(k/k-1)＝AX(k-1/k-1)进行Kalman预测，得到目标在当前帧中的位置(x1,y1)，更新状态误差协方差P(k/k-1)；其中，X(k/k-1)为使用k-1时刻状态对k时刻状态进行预测的结果，X(k-1/k-1)为k-1时刻的最优结果；

将目标的状态方程X(k)＝AX(k-1)+W(k)中的状态设置为：

其中，X(k)为k时刻系统的状态，(x(k-1),y(k-1))为k-1时刻目标的位置，移动速度分别为vx(k-1)和vy(k-1)；

步骤(c)使用前一帧的窗口宽度w和高度h，并将预测的当前帧的位置(x1,y1)作为窗口的中心，结合MeanShift跟踪方法得到目标在当前帧中的实际位置(x2,y2)；

步骤(d)使用目标在当前帧中的实际位置(x2,y2)，根据目标的测量方程Z(k)＝HX(k)+V(k)计算Kalman滤波器的观测值，计算卡尔曼增益K(k)＝P(k/k-1)H'\[HP(k/k-1)H'+R\]-1，经过Kalman状态更新校正X(k/k)＝X(k/k-1)+K(k)(Z(k)-HX(k/k-1))，得到目标的位置(x3,y3)作为目标的准确位置，同时更新状态误差协方差矩阵P(k/k)＝(1-K(k)H)P(k/k-1)，其中，P(k/k-1)是k-1时刻对k时刻的状态误差协方差的预测值：P(k/k-1)＝AP(k-1/k-1)A'+Q；

步骤(e)将目标位置(x3,y3)作为下一帧的预测位置，重复步骤(b)到步骤(d)实现目标的实时跟踪，如果关闭进程，则算法终止，否则，返回步骤(b)。

所述MeanShift跟踪方法进行目标对象跟踪的方法为：

建立的目标模型为：

其中，δ为克罗内克函数，h为窗口的带宽矩阵，k(||x||2)为核函数，b(xi)是采样点xi计算的图像特征值映射到对应的bin值得到的量化函数；

假设y为当前帧中候选目标中心的图像坐标，位于y的候选目标的模型为：

其中，m和n均表示采样数据点的数目，Cu为归一化系数：

采用巴氏距离系数来衡量目标对象模型和候选对象区域间的相似程度：

使目标对象与候选目标对象在已经选择的特征的距离空间内取得最小距离，相当于d(y)的巴氏距离系数

给出目标对象在当前图像帧的初始位置为y0，将ρ\[p(y),q\]使用一阶泰勒级数展开后得到：

定义权重系数：

得到在当前帧中的迭代位置为：

在每帧中寻找目标对象，通过使用MeanShift跟踪方法不断迭代，找到最大相似值的区域，计算出当前帧中目标的新位置y1，直到||y1-y0||＜ε停止迭代或者迭代次数达到最大值，y1成为下一帧迭达的新位置；

所述实时目标跟踪节点根据寻找的目标区域和目标区域的深度信息，计算目标与ROS移动机器人的距离，调整ROS移动机器人跟踪目标的线速度，根据目标与上位机中视觉传感器图像窗口中心的偏差，调整ROS移动机器人跟踪目标时的旋转角速度。

当没有发生遮挡时，ROS移动机器人使用实时跟踪节点进行目标跟踪，当出现完全遮挡时，ROS移动机器人使用遮挡跟踪节点进行目标跟踪；当目标对象模型和候选对象区域间的相似程度即巴氏距离系数大于0.6时，执行遮挡跟踪节点；所述遮挡跟踪节点的设计方法为：假设运动目标在视频帧中的像素点坐标为(x,y)，目标运动速度为vx和vy，图像帧更新时间为dt，建立目标的运动学方程为：

其中，ax(k-1)和ay(k-1)为k-1时刻x和y方向上的加速度，转化为：

X(k)＝AX(k-1)+W(k-1)；

其中：

建立运动目标的Kalman线性状态方程，建立测量方程为：

转化为：

Z(k)＝HX(k)+V(k)，

其中：

当发生遮挡时，使用Kalman滤波器根据前一帧的运动状态和测量值，不断预测和校正目标的位置，实现遮挡时的预测跟踪；Kalman滤波器中的状态误差协方差矩阵：过程噪声误差协方差矩阵：

所述遮挡跟踪节点在图像处理函数process\_image(self,image\_color)中根据目标丢失前的运动速度vx和vy计算目标在视频帧更新时间dt内，目标在x方向和y方向的移动距离vx\*dt和vy\*dt；再根据前一帧Kalman滤波器的校正位置(x3,y3)，使用x＝x3+vx\*dt，y＝y3+vy\*dt，得到目标在当前帧的状态X(k)＝\[x y vx vy\]T再使用测量方程得到目标的测量值；根据测量值，使用Kalman滤波器进行校正，得到目标在当前帧的位置。

本发明的有益效果：面向未知环境探索任务，实现了基于ROS系统的移动机器人自主探索和避障；基于激光雷达Rplidar A1信息建立的2D栅格地图，结合局部地图推演及全局边界搜索，设计的自主探索策略，能够避免移动机器人陷入局部探索死循环，保证能够完成整个室内环境的探索；在无需用户干预的前提下，实现对未知室内环境的自主探索与地图构建，且能够在上位机上实时显示地图构建过程，相对于传统自主探索方法构建的二维地图，直观易辨认，便于用户观察。本发明在ROS系统中利用改进Kalman滤波方法和MeanShift方法设计了实时跟踪节点和遮挡跟踪节点，解决了移动机器人目标跟踪的实时性问题和完全遮挡问题；利用Kalman滤波器的预测功能，先预测目标位置，再根据预测信息，使用Meanshift跟踪节点进行跟踪，提高了系统的运算速度，缩短了目标搜索时间，满足了跟踪的实时性要求；建立目标的状态方程和观测方程，当目标被完全遮挡时，遮挡跟踪节点根据之前的状态信息，对目标进行预测跟踪，当遮挡结束后，重新锁定目标，自动切换跟踪模式，使用实时跟踪模式对目标进行跟踪。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1为本发明的移动机器人控制结构图。

图2为ROS系统中人工势场避障方法的流程图。

图3为ROS移动机器人在模拟理想的室内环境中进行的避障实验。

图4为本发明实时跟踪节点的流程图。

图5为本发明目标跟踪系统的流程图。

图6为实时跟踪节点上位机显示的测试结果，其中，(a)、(b)、(c)和(d)分别表示运动目标远离移动机器人、运动目标靠近移动机器人以及运动目标向左和向右运动时的跟踪结果；(e)和(f)为运动目标快速向左和向右运动，实时跟踪节点的跟踪结果。

图7为使用实时目标跟踪节点进行实际测试结果，其中，(a)、(b)和(c)表示移动机器人实时跟随目标进行同步旋转；(d)、(e)和(f)表示移动机器人实时跟踪目标向前运动的过程；(g)、(h)和(i)表示运动目标靠近移动机器人的运动过程。

图8为遮挡跟踪节点上位机显示结果，其中，(a)表示目标没有被完全遮挡时，实时跟踪模式的跟踪结果；图8(b)和图8(c)表示目标被完全遮挡时，遮挡跟踪节点的跟踪结果；图8(d)表示当遮挡结束后，自动切换到实时跟踪模式进行目标跟踪。

图9为使用遮挡跟踪节点在移动机器人平台的测试结果，其中，(a)表示移动机器人使用实时跟踪模式跟踪运动目标的情况，图(b)和图(c)表示当目标发生完全遮挡时，使用遮挡跟踪模式跟踪运动目标的结果，图(d)表示当遮挡结束后，重新锁定运动目标，自动切换到实时跟踪模式进行目标跟踪的结果。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有付出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

一种基于ROS的机器人室内环境探索、避障和目标跟踪方法，其步骤如下：

步骤一：搭建ROS移动机器人的硬件平台：移动机器人的底部设置有运动控制模块，移动机器人的上部固定有传感器，移动机器人的中部设有控制器和无线通信模块，控制器上安装ROS系统，运动控制模块、传感器和无线通信模块均与控制器相连接，无线通信模块和上位机相连接。

移动机器人控制结构图如图1所示，根据控制结构图，搭建移动机器人的硬件平台：包括运动控制模块、传感器、控制器、无线通信模块和上位机，移动机器人的底部设置有运动控制模块，移动机器人的上部固定有传感器，移动机器人的中部设有控制器和无线通信模块，运动控制模块、传感器和无线通信模块均与控制器相连接，无线通信模块和上位机相连接。运动控制模块主要是Kobuki的移动底盘，传感器包括高精度的里程计、Kinect2.0深度的视觉传感器和Rplidar A1激光雷达，控制器为安装了Ubuntu14.04和ROS indigo系统的Jetson TK1开发板，无线通信模块为Intel 7260 AC HMW无线网卡。搭建完后，控制器上配置了适用于ROS系统的Kinect2.0深度视觉传感器、Rplidar A1激光雷达和无线网卡Intel 7260 ACHMW的驱动系统，以及Kobuki2.0深度视觉传感器的软件系统。无线通信模块为wifi模块。在上位机搭建了Ubuntu14.04和ROS indigo操作系统，并在操作系统中建立了自动连接的wifi模块，设置了.bashrc文件的环境变量，便于移动机器人的开发和测试。上位机通过SSH远程登录到Jetson Tk1的Ubuntu系统，启动机器人移动底盘，通过无线wifi模块使用ROS的通信方式，发布速度消息给机器人移动底盘，改变机器人的线速和角速度，控制机器人运动。

步骤二：将ROS移动机器人设置在待检测的室内，利用传感器中的激光雷达扫描室内环境，利用传感器中的里程计采集移动机器人的位置信息和方向信息，ROS移动机器人通过方波路径探索室内环境，上位机通过无线通信模块实施获取激光雷达的扫描信息，上位机中的ROS系统利用构建地图功能包构建栅格地图。

ROS系统的自主探索程序需要使用Rplidar A1激光雷达和里程计的数据，ROS系统通过订阅主题获取激光雷达和里程计发布的消息。ROS系统对获取的消息进行处理，调整Twist消息改变机器人的线速度或角速度，发布消息到ROS移动机器人的移动底盘，控制机器人按照设计的方式进行运动。

全局室内环境探索构造的拓扑地图可以减少存储的数据量，不需要存储扫描的每个位置，只存储与最后一个存储位置有一定距离的位置，但是拓扑地图过于抽象，室内自主探索和地图构建的重要任务是室内建筑的扫描，3D环境的映射需要较高的内存和计算消耗，为了简化地图构建，本发明使用栅格法进行栅格地图的构建。ROS移动机器人使用Rplidar A1激光雷达扫描室内环境信息，上位机通过wifi模块更新激光雷达实时返回的数据，上位机通过ROS系统的构建地图功能包实现栅格地图的构建。当激光雷达返回的数据中，出现障碍物时，上位机中的rviz可视化工具将栅格地图使用黑色显示出探测的障碍物的区域，没有障碍物时，使用浅灰色描述探测过的没有障碍物的区域，未探索的区域，依然显示为深灰色。

最常见的是使用漫游的方式，使机器人在室内向未知区域移动，利用激光雷达探索室内环境，反复执行，直到完成全部室内环境的探索。在搭建的ROS移动机器人平台中，首先使用漫游方式，进行室内环境探索和建图。在指定的房间中，进行漫游探索时，能够完成地图构建，但是花费时间过长，同时容易陷入局部死循环中。为了解决该问题，本发明设计出新的探索方式，在理想的室内环境中，进行方波式探索。

刚开始启动机器人时，将ROS移动机器人放置到周围1米内没有障碍物的室内地方，启动ROS系统的方波探索程序。机器人开始向未知区域移动，Rplidar A1激光雷达探测到前方出现墙壁后，当距离室内墙壁小于0.8米后，控制器控制运动控制模块开始进行转向，避开前方墙壁。转向过程中，激光雷达探测到前方没有障碍物时，ROS移动机器人开始停止旋转，重新开始向前移动，继续进行探索，此时控制器中的定时器被触发，开始计时，当移动10秒后，ROS移动机器人停止移动，按照避障时的旋转方向再旋转90度，旋转完成后继续进行移动，直到遇到墙壁时再旋转，此时的避障旋转方向和之前的旋转方向相反，重复之前步骤，直到完成室内环境探索。

在探索过程中，上位机与ROS移动机器人的操作系统通过无线通信模块进行通信，使用ROS系统中的gmapping功能包进行SLAM栅格地图构建。利用里程计计算机器人的位置，通过环境地图的特征点匹配功能实现移动机器人的精确定位，然后依据激光雷达获取的扫描数据建立局部地图，同时将全局地图进行更新。机器人从室内某个未知位置开始移动，在探索过程中使用里程计进行位姿估计，使用激光雷达数据实现自身定位，同时构建栅格地图。

ROS移动机器人自主探索室内环境进行地图构建，可以避免枯燥的机器人遥控操作。而且遥控机器人进行建图，由于人为原因，会使机器人过于靠近或远离障碍物，对于何时避障，没有明确的标准，全靠人为判断，导致建立的地图与有避障标准自主探索建立的地图有明显的差距。遥控机器人时，机器人操控速度设置的不合理，也会导致机器人离障碍物太近，无法及时旋转，碰到障碍物，影响构建地图的效果。

ROS移动机器人探索中通过人工势场避障方法进行避障，人工势场避障方法通过ROS系统的tf坐标变换，求出ROS移动机器人在全局坐标系中的位置，以及障碍物的位置；在人工势场函数中，计算总斥力与引力的合力，将合力的方向，作为机器人的避障方向。

人工势场分为引力场和斥力场，引力场即目标对象对移动机器人产生的吸引势，使机器人向目标对象所在方位运动；斥力场即障碍物对机器人产生的排斥势，可以使移动机器人远离障碍物。

对人工势场函数进行推导，首先将机器人看作一个质点，并且在二维空间中运动，假设机器人的当前位置为pc＝\[xc,yc\]T，起始点位置为ps＝\[xs,ys\]T，目标点位置为pt＝\[xt,yt\]T，利用激光雷达探测到的障碍物位置为po＝\[xo,yo\]T。

目标对ROS移动机器人形成的引力势为Ua(pc)：

障碍物对ROS移动机器人形成的斥力势为Ure(pc)：

式中λ、k、d0均为常数，其中，机器人同目标之间的欧几里得距离：机器人同障碍物之间的欧几里得距离为：

引力势和斥力势之和为U(pc)：

U(pc)＝Ua(pc)+Ure(pc)；

由引力场产生的引力为：

障碍物对ROS移动机器人的斥力：

ROS移动机器人所受虚拟势场的合力：

针对多个障碍物时，需要计算各个障碍物对机器人产生的排斥力，将多个障碍物产生的斥力合成一个总斥力。实际应用中，机器人遇到障碍物时，将合力方向作为机器人的运动方向，能够实现机器人的局部避障。在ROS系统中人工势场避障方法的流程图如图2所示，人工势场避障节点使用ROS系统的发布和订阅功能，获取激光雷达的扫描信息，当激光雷达检测到障碍物时，机器人停止移动，调用人工势场避障函数，根据里程计的位置信息，计算目标引力和障碍物斥力的合力，将合力的方向作为移动机器人的避障方向。移动机器人原地旋转到避障方向，再重新开始运动，从而实现避障。

图3为ROS移动机器人在gazebo仿真软件中，模拟理想的室内环境，进行机器人避障实验的情况。图3(a)为机器人朝向白色箱子移动，图3(b)为当机器人激光雷达探测到前方有障碍物，且距离障碍物小于设置的避障距离时，机器人停止移动，调用人工势场避障函数，计算出避障方向，然后机器人旋转到避障方向，再继续前进。图3(c)和图3(d)表示机器人避开障碍物，沿着避障方向运动时的情况。实验结果显示，机器人使用人工势场算法可以很好的进行避障。

本发明基于ROS系统和结合人工势场避障算法，设计的移动机器人对未知室内环境自主探索，构建室内栅格地图，相比传统的手动探索建图，效率更高，构建的栅格地图更接近室内环境，效果更好。相比传统的避障实验，需要提前设置好障碍物坐标和目标坐标，结合自主探索实现的人工势场避障，避障方式灵活，而且能够避免移动机器人陷入局部探索死循环，能够完成整个室内环境的探索。相比传统的移动机器人目标跟踪，我们能够使用构建的地图导航移动机器人到目标位置，进行移动机器人的目标跟踪。

当建立好地图后，上位机使用rviz可视化工具，运行Rplidar A1激光雷达的rplidar\_amcl.launch启动文件,使用map\_file或在.bashrc文件的TURTLEBOT\_MAP\_FILE环境变量中，将构建的栅格地图导入ROS移动机器人，进行目标导航和避障。在栅格地图内，机器人内进行二维位姿估计时，设置机器人的初始位姿方向后，机器人开始进行旋转，旋转到设定的方向后，停止旋转，等待任务指示。指定机器人在实际环境中的方向，使用2D目标导航，设定机器人的导航位姿；当设定导航目标后，机器开始规划路径，路径规划完成后，机器人开始沿着规划的路径朝目标移动，避开障碍物，机器人到达目标位置后，停止移动，并旋转到目标位姿方向后，停止旋转，完成导航工作的情况，到达地图内的指定位置。

移动机器人目标跟踪主要使用视觉传感器，利用设计的实时目标跟踪节点，将机器人运动控制技术同图像处理技术相结合，准确快速的跟踪选定的目标。本发明设计的实时目标跟踪节点target\_tracking.py实现了改进的目标跟踪算法，能够减少定位目标的时间，提高移动机器人跟踪目标的实时性。实时目标跟踪节点的原理主要基于Kalman滤波方法与MeanShift跟踪方法，是对Kalman滤波和MeanShift跟踪方法的合理改进，实时跟踪节点的原理如图4所示。首先建立卡尔曼对象，将卡尔曼对象的相关参数进行初始化；Kalman滤波器根据目标前一帧的位置估计当前帧Kalman的预测位置，当前帧Camshft的预测位置，然后再使用当前帧Camshft的预测位置的估计结果，对前一帧的位置进行校正。具体步骤为：

(1)首先对Kalman滤波的状态转移矩阵A、观测矩阵H、过程噪声协方差矩阵Q、测量噪声协方差矩阵R、状态误差协方差矩阵P等参数进行初始化，建立Kalman跟踪对象参数。

(2)根据前一帧的目标状态位置，使用目标的跟踪状态：X(k/k-1)＝AX(k-1/k-1)进行Kalman预测，得到目标在当前帧中的位置(x1,y1)，更新状态误差协方差P(k/k-1)；其中，X(k/k-1)为使用k-1时刻状态对k时刻状态进行预测的结果，X(k-1/k-1)为k-1时刻的最优结果。

将目标的状态方程X(k)＝AX(k-1)+W(k)中的状态设置为：

其中，X(k)为k时刻系统的状态，(x(k-1),y(k-1))为k-1时刻目标的位置，移动速度分别为vx(k-1)和vy(k-1)。

(3)使用前一帧的窗口宽度w和高度h，并将预测的当前帧的位置(x1,y1)作为窗口的中心，结合MeanShift跟踪方法得到目标在当前帧中的实际位置(x2,y2)；

(4)使用目标在当前帧中的实际位置(x2,y2)，根据目标的测量方程Z(k)＝HX(k)+V(k)计算Kalman滤波器的观测值，计算卡尔曼增益K(k)＝P(k/k-1)H'\[HP(k/k-1)H'+R\]-1，经过Kalman状态更新校正X(k/k)＝X(k/k-1)+K(k)(Z(k)-HX(k/k-1))，得到目标的位置(x3,y3)，作为目标的准确位置，同时更新状态误差协方差矩阵P(k/k)＝(1-K(k)H)P(k/k-1)，其中，P(k/k-1)是k-1时刻对k时刻的状态误差协方差的预测值：P(k/k-1)＝AP(k-1/k-1)A'+Q。

将目标位置(x3,y3)作为下一帧的预测位置，重复步骤(2)到步骤(4)实现目标的实时跟踪，如果关闭进程，则算法终止，否则，返回(2)。

实时目标跟踪节点target\_tracking.py在类TargetTracking中建立Kalman参数对象，初始化Kalman参数和Camshift参数。鼠标回调函数mouse\_cb(self,event,x,y,flags,param)，通过cv2.setMouseCallback(self.node\_name,se-lf.mouse\_cb)鼠标响应函数，可以在上位机的图像窗口手动选择跟踪目标；确定跟踪目标后，在图像处理函数process\_image(self,image\_color)中建立目标窗口的直方图，初始化目标的初始状态，通过代码实现步骤(2)到步骤(4)的功能。代码中通过随机过程噪声和当前帧Camshft获取的目标信息，使用目标的状态方程X(k)＝AX(k-1)+W(k)更新目标状态，产生随机测量噪声，通过测量方程Z(k)＝HX(k)+V(k)得到目标测量值，根据测量值，修正目标位置。

在上位机中控制机器人到达栅格地图内的指定位置后，启动机器人的目标跟踪节点，在上位机的交互界面，选定目标，让机器人跟踪目标。机器人跟踪使用Meanshift目标跟踪算法，首先确定目标模型，然后建立候选目标模型，在每帧中寻找目标对象，使用巴氏系数判断目标模型和候选模型的相似程度，相似程度越大，则候选模型越接近目标模型，寻找的区域越接近目标区域；根据寻找的目标区域和目标区域的深度信息，计算目标与机器人的距离，调整机器人跟踪目标的线速度，根据目标与上位机中Kinect2.0视觉传感器图像窗口中心的偏差，调整机器人跟踪目标时的旋转角速度。深度信息表示图像中目标距离的像素信息，转换后，用于计算机器人和目标的距离。

使用MeanShift方法进行目标对象跟踪，建立的目标模型为：

其中，δ为克罗内克函数(Kronecker delta)，h为窗口的带宽矩阵为，将候选目标对象的像素数量进行限制，k(||x||2)为核函数，b(xi)是采样点xi计算的图像特征值映射到对应的bin值得到的量化函数。目标模型能够表示出目标对象的视觉特征，图像中的特征不同，则目标模型就会有区别，对应的特征空间也相互有区别。

假设y为当前帧中候选目标中心的图像坐标，位于y的候选目标的模型为：

其中，Cu为归一化系数：

采用巴氏距离(Bhattacharyya)系数来衡量目标对象模型和候选对象区域间的相似程度：

MeanShift如果想要实现目标的跟踪，最重要的是先在图像平面中求出位置y，使目标对象与候选目标对象在已经选择的特征的距离空间内取得最小距离，相当于相似程度d(y)的Bhattacharryya系数取最大值。

给出目标对象在当前图像帧的初始位置为y0，将ρ\[p(y),q\]使用一阶泰勒级数展开后得到：

定义权重系数：

MeanShift算法得到在当前帧中的迭代位置为：

在每帧中寻找目标对象，通过使用Meanshift算法不断迭代，找到最大相似值的区域，计算出当前帧中目标的新位置y1，直到||y1-y0||＜ε停止迭代或者迭代次数达到最大值，y1成为下一帧迭达的新位置。使用巴氏系数判断目标模型和候选模型的相似程度，相似程度越大，则候选模型越接近目标模型，寻找的区域越接近目标区域。

在实时目标跟踪节点的基础上，设计了遮挡目标跟踪节点，解决移动机器人目标跟踪过程中的完全遮挡问题。移动机器人跟踪的运动目标，大部分时间的运动状态是稳定的，可以使用运动目标丢失之前的状态信息，估计遮挡时目标的运动状况进行预测跟踪，当遮挡结束后，再重新锁定运动目标。设计遮挡跟踪节点时，需要考虑判断遮挡发生的条件，当没有发生遮挡时，使用实时跟踪节点进行目标跟踪，当出现完全遮挡时，再使用遮挡跟踪节点进行目标跟踪。经过实验测试，发现目标被遮挡时，预测目标区域的颜色直方图与遮挡前目标区域的颜色直方图的巴氏距离系数会发生变化，变化范围在0～1之间。当正常跟踪时，巴氏距离会非常小，无限接近于0，当完全遮挡时，巴氏距离会非常大，无限接近于1，本发明选用0.6作为判断阈值，巴氏距离大于0.6时，执行遮挡跟踪节点。

遮挡跟踪节点occlusion\_tracking.py的设计原理：假设运动目标在视频帧中的像素点坐标为(x,y)，目标运动速度为vx和vy，图像帧更新时间为dt，建立目标的运动学方程如下：

其中，ax(k-1)和ay(k-1)为k-1时刻x和y方向上的加速度，转化为：

X(k)＝AX(k-1)+W(k-1)；

其中：

由上式可以看出，可以建立运动目标的Kalman线性状态方程，为了能够使用Kalman滤波器方法，还需要建立测量方程，假设测量方程为：

转化为：

Z(k)＝HX(k)+V(k)，

其中：

建立完运动目标的状态方程和测量方程后，可以使用卡尔曼滤波器，当发生遮挡时，根据前一帧的运动状态和测量值，不断预测和校正目标的位置，实现遮挡时的预测跟踪。

在实际测试时，系统初始状态误差协方差矩阵会影响移动机器人的遮挡跟踪效果，由于初始值难以测量，以及移动机器人平台的差异，没有经验值可供使用，且遮挡跟踪节点occlusion\_tracking.py在循环运行时，会更新其值，经过调试，发现赋予以下固定值，能够达到理想的跟踪效果：

状态误差协方差矩阵：

过程噪声误差协方差矩阵：

遮挡跟踪节点occlusion\_tracking.py在图像处理函数process\_image(self,image\_color)中根据目标丢失前的运动速度vx和vy，计算目标在视频帧更新时间dt内，目标在x方向和y方向的移动距离vx\*dt和vy\*dt；再根据前一帧Kalman滤波器的校正位置(x3,y3)，使用x＝x3+vx\*dt，y＝y3+vy\*dt，得到目标在当前帧的状态X(k)＝\[x y vx vy\]T再使用测量方程Z(k)＝HX(k)+V(k)得到目标的测量值。根据测量值，使用Kalman滤波器进行校正，得到目标在当前帧的位置。

通过ROS系统的一个循环运行命令rospy.spin()循环运行程序，不断预测和更新目标位置，实现目标的预测跟踪。并每隔一段时间，更新预测目标窗口的直方图，根据巴氏距离，判断遮挡是否结束，如果结束，且运动目标重新出现在视觉传感器的视野中，则重新锁定目标，自动切换跟踪模式，执行实时跟踪节点，恢复正常的跟踪状态。

目标跟踪系统的流程图，如图5所示。移动机器人在跟踪运动目标时，移动机器人运动节点通过订阅/roi\_zone主题，获取目标窗口，通过计算目标窗口中心与图像窗口中心的距离，设置机器人的角速度，控制机器人旋转，使机器人跟踪目标。通过订阅ROS系统架构中定义的一种功能类型的统称----kinect2/qhd/image\_color主题获取视觉传感器采集的实时图像，通过图像回调函数imageCb(self,image\_color)，处理获取的图像数据。通过订阅/kinect2/qhd/image\_depth\_rect主题获取深度图像，计算机器人和目标的距离。如果距离不在设置的阈值距离范围内，则根据实际距离与预设距离的偏差，自动调节移动机器人的线速度，通过移动机器人运动节点发布/cmd\_vel\_mux/input/navi主题，控制机器人运动，当偏差过大，则机器人快速移动，但不能超过设置的最大速度，当偏差过小，则机器人缓慢移动，但不能小于设置的最小速度，实现移动机器人速度的自动调节。可以在上位机的图像窗口观察移动机器人的目标跟踪情况。正常情况下，移动机器人使用实时跟踪模式跟踪目标，发生遮挡时，切换到遮挡跟踪模式进行预测跟踪，当遮挡结束后，重新捕捉到运动目标后，再切换回实时跟踪模式。两种模式跟踪目标时，移动机器人都会自动调节，与运动目标保持安全的跟踪距离。当目标停止时，移动机器人自动进行微调，停在安全距离范围内。

为了验证和展示实时跟踪节点的跟踪效果，分别对上位机的显示结果和移动机器人平台的测试结果进行分析。

实时跟踪节点上位机显示的测试结果，如图6所示。图6(a)、图6(b)、图6(c)和图6(d)分别表示运动目标远离移动机器人、运动目标靠近移动机器人以及运动目标向左和向右运动时的跟踪结果，倾斜的矩形为实时跟踪节点搜索到的目标的实际区域，没有倾斜的矩形为预测的目标区域。实验结果表明，实时跟踪节点的Kalman滤波器能够准确的预测到目标的运动方位，使MeanShift能够快速的搜索到运动目标，自适应调整目标窗口实现目标的实时跟踪。测试运动目标快速移动时，仅使用倾斜的矩形标记目标区域。图6(e)和图6(f)为运动目标快速向左和向右运动时，实时跟踪节点的跟踪结果。实验结果表明，运动目标瞬时速度发生巨大变化时，视频帧中运动目标图像变得模糊，目标形状发生变化，但是实时跟踪节点依然能够实时捕捉到运动目标，实现实时跟踪。

移动机器人进行实际测试时，需要根据实验环境，手动设置相关参数。实时跟踪测试中，安全距离阈值为0.65米，最大旋转速度为1.2rad/s，最小旋转速度为0.2rad/s，最大线速度0.5m/s，最小线速度0.05m/s。

图7为使用实时目标跟踪节点进行实际测试的结果，图7(a)、图7(b)和图7(c)表示移动机器人实时跟随目标进行同步旋转；图7(d)、图7(e)和图7(f)表示移动机器人实时跟踪目标向前运动的过程，当目标远离机器人，大于设置的安全距离时，移动机器人开始跟踪目标向前运动，运动目标停止运动后，移动机器人开始自动调整，停在安全距离范围内；图7(g)、图7(h)和图7(i)表示运动目标靠近移动机器人的运动过程，当运动目标靠近移动机器人，低于设定的安全距离时，移动机器人开始向后倒退，跟踪运动目标，自动与运动目标保持安全的跟踪距离，直到运动目标停止移动，移动机器人自动调整到合适的位置后停止运动。实际测试过程中，运动目标正常的移动或突然改变速度时，移动机器人依然能够稳定的进行跟踪，说明设计的实时跟踪节点能够满足移动机器人实时跟踪目标的要求。

为了更好的验证实时跟踪节点的性能，将传统的MeanShift跟踪方法，设计成MeanShift跟踪节点，与实时跟踪节点进行比较。根据两种跟踪节点在视频帧中搜索到运动目标的时间，验证两种节点的运算性能，如表1所示。虽然实时跟踪节点增加了程序步骤，但是使用Kalman方法进行预测，减少了迭代次数，缩短了搜索时间。由表1可知，实时跟踪节点能够更快的搜索到运动目标，结束本次的迭代运算，平均时间在0.001秒左右，相比传统的MeanShift跟踪节点快了0.008秒左右。说明使用ROS系统设计的实时跟踪节点性能的优越性，满足了移动机器人目标跟踪的实时性要求。

表1传统的MeanShift跟踪和实时跟踪运算性能比较

遮挡跟踪节点在实时跟踪模式的基础上，增加了遮挡跟踪模式。遮挡跟踪节点上位机显示结果如图8所示。

图8(a)表示目标没有被完全遮挡时，实时跟踪模式的跟踪结果，实验表明，半遮挡时，实时跟踪模式依然能够很好的进行目标跟踪；图8(b)和图8(c)表示目标被完全遮挡时，遮挡跟踪节点的巴氏距离大于设置的阈值0.6，自动切换到遮挡跟踪模式，进行遮挡预测，预测目标的运动方向，实现预测跟踪，没有倾斜的矩形表示预测的目标位置，实验表明，遮挡跟踪模式根据遮挡之前目标状态的先验信息，能够准确的预测出目标运动方向；图8(d)表示当遮挡结束后，重新锁定运动目标，结束遮挡跟踪模式，自动切换到实时跟踪模式进行目标跟踪。实验表明，设计的遮挡跟踪节点能够很好的进行目标的遮挡跟踪，提高了跟踪系统的鲁棒性。

图9为使用遮挡跟踪节点，在移动机器人平台的测试结果，图9(a)表示移动机器人使用实时跟踪模式跟踪运动目标的情况，图9(b)和图9(c)表示当目标发生完全遮挡时，使用遮挡跟踪模式跟踪运动目标的结果，图9(d)表示当遮挡结束后，重新锁定运动目标，自动切换到实时跟踪模式进行目标跟踪的结果。图9中当运动目标朝白色挡板移动时，实时跟踪模式驱动移动机器人同步跟踪运动目标，运动目标被完全遮挡时，遮挡跟踪模式预测目标运动方向，移动机器人按照预测的目标方向进行运动，遮挡结束后，重新锁定运动目标，切换到实时跟踪模式，移动机器人继续对目标进行实时跟踪。实验表明，设计的遮挡跟踪节点能够很好的解决一定条件下移动机器人目标跟踪的遮挡问题，取得理想的跟踪效果。

本发明基于ROS(Robot Operating System)机器人操作系统的移动机器人目标跟踪系统的实现，针对移动机器人目标跟踪实时性差的问题，设计的实时目标跟踪节点，根据目标的先验信息，利用Kalman滤波预测目标位置，MeanShift在预测信息的基础上搜索目标，提高了系统的运算速度，缩短了目标搜索时间，满足了系统的实时性要求。针对目标跟踪完全遮挡问题，建立目标的状态方程和观测方程，设计的遮挡跟踪节点，根据遮挡前目标的状态信息，对目标进行预测跟踪，遮挡结束后，重新锁定目标，自动切换回实时跟踪模式。实验结果验证了跟踪系统的实时性和鲁棒性。

本发明使用ROS系统，利用Kalman滤波器的预测功能和MeanShift方法基于颜色特征的跟踪功能，将跟踪方法灵活使用，与硬件和软件结合，设计了实时跟踪节点和遮挡跟踪节点，解决了改装的Turtlebot2移动机器人目标跟踪过程的实时性问题和完全遮挡问题。实验结果和实验数据表明，设计的跟踪节点，实现了移动机器人对目标的实时跟踪，能够满足一定条件下移动机器人的目标跟踪需要，提高了目标跟踪系统的鲁棒性和稳定性。

以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。