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系统，运动控制模块、传感器和⽆线通信模块均与控制器相连接，⽆线通信模块和上位机相连接
2012人均gdp步骤⼆：将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系统和结合⼈⼯势场避障算法，设计的移动机器⼈对未知室内环境⾃主探索，构建室内栅格地图，相⽐传统的⼿动探索建图，效率更⾼，构建的栅格地图更接近室内环境，效果更好。相⽐传统的避障实验，需要提前设置好障碍物坐标和⽬标坐标，结合⾃主探索实现的⼈⼯势场避障，避障⽅式灵活，⽽且能够避免移动机器⼈陷⼊局部探索死循环，能够完成整个室内环境的探索。相⽐传统的移动机器⼈⽬标跟踪，我们能够使⽤构建的地图导航移动机器⼈到⽬标位置，进⾏移动机器⼈的⽬标跟踪。
步骤三：将建⽴好的栅格地图导⼊ROS移动机器⼈的ROS系统中，使⽤构建的栅格地图导航机器⼈到地图内的指定位置，视觉传感器基于Kalman滤波⽅法与MeanShift跟踪⽅法实现⽬标的跟踪。
豫公网安备41160202000603
站长QQ:729038198
关于我们
投诉建议