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

本发明涉及机器人运动的技术领域，尤其涉及一种基于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、测量噪声协方