CN103926925B - 一种基于改进的vfh算法的定位与避障方法及机器人 - Google Patents

Abstract

本发明提出了一种基于改进的VFH算法的定位与避障方法及机器人。基于改进的向量场直方图法和扫描匹配算法,利用激光测距器传感器来获得环境信息，利用极坐标扫描匹配算法来修正里程计所带来的位姿误差。在完成机器人定位后，将环境信息栅格化，根据机器人与障碍物之间的关系，考虑了移动机器人的传感不确定性和机器人的实际大小，对障碍物进行膨胀，建立初始的极坐标直方图，获得自由行走区和避障区，通过定义两个阈值获得二元极坐标直方图，通过估计移动机器人的运动轨迹，建立一个遮挡极坐标直方图，最后引入代价函数来确定机器人的最佳运动方向来解决家庭环境下移动机器人的避障路径规划问题。

Description

一种基于改进的VFH算法的定位与避障方法及机器人

技术领域

本申请涉及智能机器人自主导航领域，具体涉及机器人的地创建和同时定位领域，尤其涉及一种基于改进的VFH算法的定位及避障方法及机器人。

背景技术

智能机器人，例如，扫地机器人、机器人越来越广泛地应用于家庭生活中，机器人要实现灵活、高效、智能地移动，需要具有自主导航能力。地图创建(MapBuilding)、定位(Location)和路径规划(Path Planning)是自主导航的三个关键要素。本发明主要涉及地图创建与同时定位领域。其中，地图创建与定位是相互依存的关系，缺少环境地图则无法准确标定机器人的位置，初始位置不确定，则创建的地图缺少基准点。正因如此，未知环境下机器人的定位与地图创建将以同时定位与地图创建的方式来实现，即移动机器人随着对环境的探索，逐步扩大自身存储的地图的广度，并实时的将位置信息标定在新创建的地图中。这种技术一般称之为同时定位与地图生成(SLAM，Simultaneouslocalization and Mapping)。目前，较为常用的智能机器人的SLAM技术实现包括FastSLAM与vSLAM(visual SLAM)两大类。其中，FastSLAM***一般使用激光测距仪或声纳来实现，而vSLAM则使用视觉传感器来实现。FastSLAM由于使用了激光、声纳等传感器，对一些特殊的环境信息，如线段、拐角等并不能识别其特殊意义，因此需要通过改进算法来提高定位的准确性。

目前较为常见的移动智能机器人定位技术主要是根据先验的环境信息，结合当前的机器人位置信息以及传感器输入信息，准确地确定机器人位姿的过程。主要包括相对定位与绝对定位，绝对定位主要采用导航信标、主动或被动标识、地图匹配或卫星导航技术(GPS)进行定位,定位精度较高,但对于家用机器人而言成本较高；相对定位是通过测量机器人相对于初始位置的距离和方向来确定机器人的当前位置,通常也称为航位推算法,常用的传感器包括里程计及惯导***，例如速度陀螺、加速度计等。航位推算法的优点是机器人的位姿是自我推算出来的,不需要对外界环境的感知信息,缺点是漂移误差会随时间累积，我们知道任何小的误差经过累积都会无限的增加，因此需要考虑误差修正。

现有技术中，已经对相关技术进行了各种探究，但主要集中在各个专用领域的子***。例如，发明专利申请CN103455034A公开了一种基于最近距离向量场直方图的避障路径规划方法，该方法将机器人当前扫描范围均分为n个扇区，基于最近距离向量场直方图的避障路径来规划方法；发明专利申请CN102541057A公开了一种基于激光测距仪的移动机器人避障方法，通过将激光信息分组,在每组中选择障碍点，将障碍点映射到机器人坐标系中，采用速度控制策略给出机器人线速度和角速度，该发明在未知环境中能有效地避障,性能优良、实用性强，特别适合于实际应用场合；发明专利申请CN103439972A公开了一种动态复杂环境下的移动机器人路径规划方法，利用栅格法得到栅格地图，栅格法表示的障碍物分布图转化为图的赋权邻接矩阵，采用蚁群算法对环境进行全局路径规划,并使用退步法则处理环境中的陷阱问题；发明专利申请CN101943916A公开了一种基于卡尔曼滤波器预测的机器人避障方法，当传感器***探测到有新障碍物出现,根据观测数据建立卡尔曼滤波器模型,利用观测数据和经典的线性动态***期望最大化模型辨识算法对参数进行辨识和修正,更新数字地图,供路径规划器进行新一轮的局部重规划；发明专利申请CN103092204A公开了一种混合的机器人动态路径规划方法，该方法能够应用在环境信息部分已知且同时存在未知的动态和静态障碍物的情况下，用一种遗传算法作为全局规划方法得到全局路径,再用改进的人工势场法进行局部规划。

上述方法均有效提高了机器人导航定位的精确度，但仍然存在各种问题。在此基础上，本发明提出了一种基于改进的VFH算法的定位与避障方法及采用该方法进行定位及避障的机器人。基于改进的向量场直方图法和扫描匹配法利用激光测距器传感器来获得环境信息，利用极坐标扫描匹配算法来修正里程计所带来的位姿误差。在完成机器人定位后，将环境信息栅格化，根据机器人与障碍物之间的关系，考虑了移动机器人的传感不确定性和机器人的实际大小，对障碍物进行膨胀，建立初始的极坐标直方图，获得自由行走区和避障区，通过定义两个阈值获得二元极坐标直方图，通过估计移动机器人的运动轨迹，建立一个遮挡极坐标直方图，最后引入代价函数来确定机器人的最佳运动方向来解决家庭环境下移动机器人的避障路径规划问题。

发明内容

本发明的目的是提供一种基于改进的VFH算法的定位与避障方法及采用该方法进行定位及避障的机器人。基于改进的向量场直方图法和扫描匹配法利用激光测距器传感器来获得环境信息，利用极坐标扫描匹配算法来修正里程计所带来的位姿误差。在完成机器人定位后，将环境信息栅格化，根据机器人与障碍物之间的关系，考虑了移动机器人的传感不确定性和机器人的实际大小，对障碍物进行膨胀，建立初始的极坐标直方图，获得自由行走区和避障区，通过定义两个阈值获得二元极坐标直方图，通过估计移动机器人的运动轨迹，建立一个遮挡极坐标直方图，最后引入代价函数来确定机器人的最佳运动方向来解决家庭环境下移动机器人的避障路径规划问题。

本发明公开了一种基于改进的VFH算法的定位与避障方法,其特征在于，包括以下步骤：

环境障碍检测，利用激光测距器对周围环境进行扫描，并对机器人进行定位；

环境信息栅格，采用向量映射法建立环境栅格地图；

栅格障碍加权，赋予每个栅格内的点不同的权重；

活动窗口分区，通过活动窗口对向量化后的栅格进行分区；

计算获取最近距离向量极坐标直方图；

建立避障区和自由行走区；

若存在自由行走区，则控制机器人向后选取方向运动。

本发明还公开了一种基于上述方法进行定位的机器人，所述机器人包括一感知***、核心控制模块、人机交互***、电机驱动***，所述核心控制模块对机器人内部各单元进行控制，并根据感知***反馈的信息，以及外界获得交互信号控制电机驱动***，以控制机器人的移动。

附图说明

图1是本发明所述机器人的组成结构图；

图2是本发明所述基于改进向量场直方图的避障路径规划模块的框图；

图3本发明所述障碍扩大示意图；

图4是本发明所述建立避障区和自由行走区示意图

具体实施方式

为使本发明的目的、技术方案和优点更加清楚明了，下面结合具体实施方式并参照附图，对本发明进一步详细说明。应该理解，这些描述只是示例性的，而并非要限制本发明的范围。此外，在以下说明中，省略了对公知结构和技术的描述，以避免不必要地混淆本发明的概念。

如图1所示，本发明主要涉及家庭环境中的智能机器人，所述机器人是具有环境感知，同时定位与地图创建、避障路径规划的自主移动机器人。包括一感知***、核心控制模块、人机交互***、电机驱动***。所述感知***接受外界多种途径输入的音频和/或视频信号，以及其它能感知外界状态的信号以及定位信息。这些信号或信息来自于激光测距模块，以及可以来自环境感知模块、语音采集模块、视频采集模块、超声波测距模块、里程计信息模块等模块的一个或多个模块。根据需要，还可以设置有障碍物检测模块、避障路径规划模块等。通过获取在普通家庭的环境内感知到的一项或多项如下信息，从而感知周围环境的视觉信息、检测到行走路线周围的障碍。

智能机器人通过人机交互***和/或无线收发模块实现与外界的交互。

人机交互***顾名思义用于外界与机器人进行交互，这种交互是可以根据实际需求进行设置的，例如人工控制机器人的工作状态，路径，设置相应的参数、模式等等。所述参数可以是时间参数，频率参数，速度参数等，所述模式包括跟随模式，巡逻模式和异常行为处理模式。人机交互***还可以通过设置在机器人上的显示屏或信号灯指示机器人的工作状态。

智能机器人还可以通过无线收发模块接受来自移动终端，例如智能手机的信号，从而实现人机交互。所述人机交互***和/或无线收发模块均与核心控制模块连接。

所述核心控制模块对机器人内部各单元进行控制，并根据感知***反馈的信息，以及外界获得交互信号控制电机驱动***，以控制机器人的移动。

其中所述电机驱动***包括底盘控制器、电机驱动器，并装有移动所需的电池模块、自主充电功能模块、轮子等。其中，所述轮子为2个驱动轮和1个万向轮。核心控制模块通过串口向底盘控制器发送控制命令，控制电机驱动器进行相应动作，并对障碍信号进行处理。

本申请仅给出了电机驱动***的一种实施方式，但本领域技术人员应该知晓，任何通过电机驱动以实现机器人移动的方式，对本申请而言都是显而易见的。

本申请所公开的两轮驱动轮和一个万向轮的结构可以使得机器人能够实现零半径转向，前进后退左右转动等各种运动功能。激光测距器是激光测距定位模块的一个部分，激光测距器是利用激光技术进行测量的传感器，能实现无接触远距离测量，速度快，精度高，量程大，抗光、电干扰能力强等，

将实时采集的激光测距器数据传输到机器人内部的上位机进行处理。根据所获取的激光测距器数据，采用本发明所公开的基于改进的VFH算法的定位与避障技术感知周围环境，完成机器人的自主定位，使机器人能够在家庭环境中自主导航并完成更多的辅助人类生活的功能。

如图2所示，本发明所述基于改进的VFH算法的定位与避障方法包括以下步骤：

环境障碍检测，利用激光测距器对周围环境进行扫描，并对机器人进行定位；

环境信息栅格，采用向量映射法建立环境栅格地图；

栅格障碍加权，赋予每个栅格内的点不同的权重；

活动窗口分区，通过活动窗口对向量化后的栅格进行分区；

计算获取最近距离向量极坐标直方图；

建立避障区和自由行走区；

若存在候选区，则控制机器人向后选取方向运动。

所述定位是基于激光测距器的服务机器人的扫描匹配定位方法，其主要流程包括以下步骤：

预处理步骤，将当前的扫描数据进行预处理，滤掉噪声点；

预匹配步骤，将当前的扫描数据与预存的扫描数据进行预匹配，当前的扫描数据与预存的扫描数据的数据集之间的估计匹配值由里程计给出；

选择步骤，从每个扫描数据集中选取若干个匹配点；

点的匹配步骤，将当前的扫描数据的数据集中选择的点与预存的扫描数据的数据集中选择的点进行匹配，形成若干对应点对；

加权步骤，赋给每个对应点对一个权重；

剔除步骤，将扫描数据集中那些从当前机器人位置无法看到的点剔除，并通过预先定义的阈值来剔除出格点；

误差度量计算步骤：采用从一个扫描数据集中的点到另一个扫描集合中的点之间的最小距离的平方和作为误差度量计算，并通过扫描匹配算法进行匹配。

确定坐标步骤：根据扫描匹配算法的结果判断机器人所处的位置。

本发明所述基于改进的VFH算法的定位与避障方法是在上述基于激光测距器的扫描匹配定位算法的基础上提出了一种基于SLAM的改进向量场直方图算法，用来解决移动机器人在密集复杂环境中的避障和路径规划问题。VFH算法，是将机器人的工作环境分解为一系列具有二值信息的栅格单元，每个矩形栅格中有一个积累值，表示在此处存在障碍物的可信度，高的累计值表示存在障碍物的可信度高。这是因为传感器不断快速的采样环境，存在障碍物的栅格不断被检测的结果。栅格大小的选择直接影响着控制算法的性能。栅格选得小，环境分辨率就高，但是抗干扰性就比较弱，环境信息存储量大，使得决策速度慢；栅格选的大，抗干扰性就比较强，但环境分辨率下降，在密集障碍物环境中发现路径的能力减弱。另外，栅格大小的选取也与传感器的性能有关，若传感器的精度高而且反应速度快，栅格可以选的小些。在VFH算法中用二维的栅格来表征环境。将机器人的工作空间划分为若干连续的二维栅格系列。每个栅格中都包含一个概率值(CV值)。这个概率值体现了在该栅格中存在障碍物的可信度，CV(Certainty Value)值越高，表示此处存在障碍物的可能性就越大，由此，传感器具有不确定性。

本发明采用基于激光测距数据的扫描匹配算法完成机器人任意时刻的同时定位与地图创建问题，并采用向量映射法有效地减少建立环境栅格地图的计算量；根据机器人与环境地图中障碍物之间的关系，考虑了移动机器人的传感不确定性和机器人的实际大小，对障碍物进行膨胀，建立障碍点集，通过相邻障碍物的融合形成了障碍边界集，并以此建立初始的距离向量极坐标直方图，获得自由行走区和避障区；通过定义一个时变的阈值函数获得二元极坐标直方图；通过估计移动机器人的运动轨迹，建立一个遮挡极坐标直方图，将机器人的运动学和动力学影响遮挡掉；根据遮挡极坐标直方图及代价函数来选择最佳的运动方向角，避开障碍物，驶向目标点，以便其在室内结构化环境下自主导航。

如图3所示，本发明所述机器人在任何时刻对外界环境的感知范围都是有限的，并取决于所用传感器的有效测量范围。定义某一时刻机器人所能感知的最大范围为活动窗口，它实际上是以机器人为圆心，激光测距器所测范围为半径的圆形区域。采用向量映射法建立环境栅格地图具体该向量大小由下式确定：

m_i,j＝(c_i,j ^*)²(a-bd_i,j)

而其方向取决于栅格与机器人中心点(VCP)的相对位置：

${\mathrm{\β}}_{i,j}={\tan }^{-1}\frac{y_j-y_0}{x_i-x_0}$

其中：a，b是正常数；

c_i,j ^*活动窗口内栅格(i,j)的CV值

d_i,j该栅格到机器人中心点(VCP)的距离值

x₀；y₀此刻机器人中心点(VCP)的绝对位置坐标。

x_i；y_j该栅格的绝对位置坐标

(二)活动窗口分区

若选择角分辨率α，则分区后得到的区间总数n＝360/α。对于任意区间k,(k＝0,1,2,…,n-1)，有k＝int(β_i,j/α)。其障碍密度h_k可以由下式得出：在本研究中α＝5。

由于CV值的离散性，可能导致障碍密度的过于稀散。因此要对其进行平滑处理：

${h^{\text{'}}}_k=\frac{h_{k-l+1}+{2h}_{k-l+2}+\·\·\·+{\mathrm{lh}}_k+(l-1)h_{k+1}+\·\·\·{2h}_{k+l1}+h_{k+l}}{2l-1}$

(三)确定运动方向θ

给定某一阈值τ，障碍密度低于该值的区域，称为“候选区”。当有连续S_max个候选区存在时，称它们为“宽谷”；否则称之为“窄道”。将这些连续候选区中最左边的一个区记为k_l，最右边的一个区记为k_r，则运动方向可以由下式子得出： $\mathrm{\θ}=\frac{1}{2}(k_1+k_r)$

充分考虑了机器人的大小尺寸对算法结果的影响。在将栅格放大r_r，r_r取决于机器人的尺寸。为了进一步加强机器人运行的安全性，还要将机器人与障碍可以保持不碰撞的最短距离d_s考虑在内。所以对于任意待研究的栅格其实际上被放大了r_r+s，r_r+s＝r_r+d_s。

为了动态分析机器人的运动，将其轨迹分解成运动方向不变时的直线与方向改变时的弧线。该弧线取决于机器人的转动半径，与机器人的速度密切相关，速度越快，其旋转半径越大。如图4所示，假设机器人向左旋转时的转向半径为r_left，右转时的转向半径为r_right。A，B为两个障碍栅格。按上述方法将A,B扩大，假定A与机器人的左转向圆有重叠交叉，那么A与左转向圆覆盖的所有区域被认为是避障区(blocked)；B与转向圆无交叉现象所以只有其自身覆盖的区域为避障区。在图4所示情况下，机器人将右转。

通过对上述条件的对照比较可以得到两个极限角度，分布在机器人左右两侧。分别记为及同时定义表示机器人当前运动方向的反向。初始时刻令对于活动窗口中任意栅格C_i,j，在其CV值满足CV＜τ的情况下若β_i,j位于左侧、θ右侧时，令若β_i,j位于θ左侧、右侧时，令这样，根据及可以得到另一形式的柱状图(masked polarhistogram)。该柱状图表明了机器人在当前速度下的可行方向。若且 表示该区域可行；其他情况下，表示该区域内不可行。

通常情况下，我们可以得到若干符合组合。对于每个组合，记其左右边界分别为：k_l和k_r。若k_l和k_r间包含了S_max以上个区间(S_max为常量，本实验中取10)，则称该区域为“宽域”；否则称之为“窄域”。窄域只提供一个候选方向转化为角度后为而宽域可以提供三个候选方向：c_r，c_l以及c_t。

$c_r=k_r+\frac{S_{\max }}{2},$ 转化成角度为

$c_l=k_1-\frac{S_{\max }}{2},$ 转化成角度为

此外，当目标方向k_t满足k_t∈[c_r,c_l]时有c_t＝k_t，转化成角度为为选出最合适的运动方向c，建立以下代价函数：

$g\left(c\right)={\mathrm{\μ}}_1\·\mathrm{\Δ}(c,k_t)+{\mathrm{\μ}}_2\·\mathrm{\Δ}(c,\frac{{\mathrm{\θ}}_i}{\mathrm{\α}})+{\mathrm{\μ}}_3\·\mathrm{\Δ}(c,k_{n,i-1}),$ 其中：

Δ(c₁,c₂)＝min{c₁-c₂||,c₁-c₂-n||,c₁-c₂+n|}，是用来计算两个区间c₁和c₂绝对角度差的函数。比如Δ(c,k_t)表示的是候选方向与目标方向间的差角，其值越大，机器人运动时偏离目标就越远，到达目的地的代价也就越高。表示的是候选方向与机器人行进方向间的角度差，该值越大，机器人转向变化越大。Δ(c,k_n,i-1)表示的是当前候选方向与前次所选方向间的角度变化，值越大，车轮转向变化越大，运动震荡越大。

所以权衡系数μ₁，μ₂及μ₃的选择至关重要，它们直接决定了算法的优越性。一般情况下应满足：μ₁＞μ₂+μ₃。在本实施例中，μ₁＝5，μ₂＝μ₃＝2。

本发明通过安装在家庭服务机器人前面的激光测距器获得机器人运行路径周围的环境信息，使用方位角匹配规则消除对应点的搜索问题，使得计算平移复杂度降为O(n)，并在服务机器人内嵌的里程计数据的帮助下，对机器人的当前位姿进行估计，同时进行全局一致性地图的生成，完成家庭服务机器人的同时定位与地图创建(SLAM)。在家庭服务机器人解决其SLAM问题后，进一步将机器人的工作环境分解为一系列栅格单元，考虑到传感器测量的不确定性和机器人的大小，对探测到的每个障碍物栅格点膨胀，接着根据机器人与环境中障碍物之间的关系，建立初始的极坐标直方图，获得自由行走区和避障区间，通过定义两个阈值获得二元极坐标直方图，考虑到机器人的运动学和动力学约束，在二元极坐标直方图基础上建立遮挡极坐标直方图，最后根据遮挡极坐标直方图及代价函数来选择最佳的运动方向角，避开障碍物，驶向目标点，以便其在室内结构化环境下自主导航。

对于本领域技术人员而言，显然本发明不限于上述示范性实施例的细节，而且在不背离本发明的精神或基本特征的情况下，能够以其他的具体形式实现本发明。因此，无论从哪一点来看，均应将实施例看作是示范性的，而且是非限制性的，本发明的范围由所附权利要求而不是上述说明限定，因此旨在将落在权利要求的等同要件的含义和范围内的所有变化囊括在本发明内。

此外，应当理解，虽然本说明书按照实施方式加以描述，但并非每个实施方式仅包含一个独立的技术方案，说明书的这种叙述方式仅仅是为清楚起见，本领域技术人员应当将说明书作为一个整体，各实施例中的技术方案也可以经适当组合，形成本领域技术人员可以理解的其他实施方式。

Claims (10)

1.一种基于改进的VFH算法的定位与避障方法,其特征在于，包括以下步骤：

环境障碍检测，利用激光测距器对周围环境进行扫描，并对机器人进行定位；

环境信息栅格，采用向量映射法建立环境栅格地图；

栅格障碍加权，赋予每个栅格内的点不同的权重；

活动窗口分区，通过活动窗口对向量化后的栅格进行分区；

计算获取最近距离向量极坐标直方图；

建立避障区和自由行走区；

若存在候选区，则然后控制机器人选取方向运动，定义两个阈值获得二元极坐标直方图，并在二元极坐标直方图基础上建立遮挡极坐标直方图，最后根据遮挡极坐标直方图及代价函数来选择最佳的运动方向角，避开障碍物，驶向目标点。

2.如权利要求1所述的方法，其特征在于，所述对机器人进行定位包括以下步骤：

1）         预处理步骤，将当前的扫描数据进行预处理，滤掉噪声点；

2）         预匹配步骤，将当前的扫描数据与预存的扫描数据进行预匹配，当前的扫描数据与预存的扫描数据的数据集之间的估计匹配值由里程计给出；

3）         选择步骤，从每个扫描数据集中选取若干个匹配点；

4）         点的匹配步骤，将当前的扫描数据的数据集中选择的点与预存的扫描数据的数据集中选择的点进行匹配，形成若干对应点对；

5）         加权步骤，赋给每个对应点对一个权重；

6）         剔除步骤，将扫描数据集中那些从当前机器人位置无法看到的点剔除，并通过预先定义的阈值来剔除出格点；

7）         误差度量计算步骤：采用从一个扫描数据集中的点到另一个扫描集合中的点之间的最小距离的平方和作为误差度量计算，并通过扫描匹配算法进行匹配；

8）         确定坐标步骤：根据扫描匹配算法的结果判断机器人所处的位置。

3.如权利要求1所述的方法，其特征在于，所述栅格障碍加权主要包括：

每个栅格中都包含一个概率值，所述概率值体现在该栅格中存在障碍物的可信度，可信度值越高，表示此处存在障碍物的可能性就越大。

4.如权利要求1所述的方法，其特征在于，所述活动窗口分区是根据机器人与环境地图中障碍物之间的关系，基于机器人的实际大小，对障碍物进行膨胀，建立障碍点集，通过相邻障碍物的融合形成了障碍边界集。

5.一种采用权利要求1所述方法进行定位的机器人，所述机器人包括一感知***、核心控制模块、人机交互***、电机驱动***，所述核心控制模块对机器人内部各单元进行控制，并根据感知***反馈的信息，以及外界获得交互信号控制电机驱动***，以控制机器人的移动。

6.根据权利要求5所述的机器人，其特征在于，所述机器人还包括激光测距模块，以及环境感知模块、语音采集模块、视频采集模块、超声波测距模块、里程计信息模块中的一个或多个模块；感知***接收所述一个或多个模块的信号。

7.根据权利要求6所述的机器人，其特征在于，所述机器人根据需要，还可以设置有障碍物检测模块、避障路径规划模块。

8.根据权利要求5所述的机器人，其特征在于，所述机器人通过人机交互***设置工作模式，所述模式包括跟随模式，巡逻模式和异常行为处理模式。

9.根据权利要求5所述的机器人，其特征在于，智能机器人可以通过无线收发模块接受来自移动终端的信号，从而实现人机交互。

10.根据权利要求5所述的机器人，其特征在于，其中所述电机驱动***包括底盘控制器、电机驱动器，并装有移动所需的电池模块、自主充电功能模块、轮子；其中，所述轮子为2个驱动轮和1个万向轮，核心控制模块通过串口向底盘控制器发送控制命令，控制电机驱动器进行相应动作，并对障碍信号进行处理。