复合边界点驱动的未知三维环境探索路径规划方法研究

唐嘉宁, 刘雨晴, 周思达, 李丁奎

(云南民族大学 电气信息工程学院, 云南 昆明 650504)

近年来,无人机的应用领域越来越广泛,特别是在军事海事、运输投送、应急救援等方面。在这些任务中,理想结果是无人机能快速而又充分覆盖给定的未知区域,但当无人机处于室内无全球定位系统(GPS)环境时,其定位导航困难,无法进行有效自主探索,故本文通过RGBD传感器对无人机进行定位以完成探索任务。在过去的几十年里,研究者们已经提出了许多方法,包括基于边界的方法和基于采样的方法。

基于边界的方法通过探索地图已知区域和未知区域之间的边界来完成探索未知环境的任务。最早是由Yamuchi[1]于1997年提出的,这是目前大多数探索算法的基础,边界定义为开放空间和未开放空间之间边界上的区域,机器人移动到边界可以看到未开放的空间,并将新信息添加到地图上,但该方法存在无人机探索时间长且效率低的问题。Zhou等[2]为提高探索效率,提出一个支持快速探索的分层框架FUEL,通过前沿信息结构维护空间所需信息，可找到有效的全局覆盖路径。Gomez等[3]将基于前沿的概念与基于行为的策略相结合以构建环境的拓扑表示,根据成本效用函数进行语义边界分类和边界选择。Faria等[4]为解决同时执行自主探索和路径规划,将基于边界的探索与Lazy Theta*路径规划算法结合在八叉树地图上,有效减少了寻找探索前沿单元所需的迭代次数。

基于采样的方法则随机产生机器人状态并搜索路径。Umari等[5]提出用快速拓展随机树(RRT)检测边界点,其局部RRT进行了一步树的重置工作,每次重置树都会从一个新的初始点开始生长,对计算资源要求高。Qiao等[6]为减少内存消耗以及在不利环境中探索,提出用分块结构判断树节点是否在前沿检测中起决定性作用,从而删除大量冗余的树节点。Bircher等[7]采用了滚动水平的次最佳视图(next-best-view)方案,在在线计算的随机树中找到最佳分支,每个规划步骤只执行此分支的第一条边,但在视场中选择次最佳可能导致次优解,且重复扩展RRT导的前提下,分层规划器分步规划探索以导致计算量大。Dornhege等[8]为解决选择次优视图的问题,通过引入空隙和边界,将基于二维前沿的勘探方法扩展到三维环境。

如何选取自主探索的目标点在无人系统自主探索环境中起到决策作用。Yamuchi利用广度优先搜索方法寻找离当前位置最近的点,导致无人机会反复探索同一个区域,增加探索路径长度。Ibrahim等[9]为提高基于边界的方法的搜索速度,提出了一种基于遗传算法的全局搜索目标分配方法, 通过遗传算法启发式地生成可能访问的所有边界的路线,迭代几次后选择最短路线的第一个边界点作为下一个目标位置。Liang等[10]使用聚类算法过滤从八叉图中提取的边界,并使用基于信息增益的代价函数来选择最优边界,可节约探索时间但是算法复杂度高。Batinovic等[11]通过改变八叉树图中点的分辨率过滤边界点,使用均值漂移聚类减少边界点数量并选择要探索的最佳边界点。Fang等[12]通过优化RRT算法生成的随机前沿点,提出随机前沿点优化算法,将该算法与前沿点评价函数相结合,选择有高估值的前沿点作为目标前沿点。

综上所述,探索是由一个或一组机器人在合理的时间内获得尽可能多的关于周围的信息,达到覆盖未知环境的过程。然而,大多数方法的探索效率都不高,而且为了保证获取尽可能高的信息增益以及提高在未知环境下无人机飞行的安全性,大多数方法都是在低速条件下进行,这在解决短时间内获取更多的信息方面具有局限性。受文献[13]启发,在高速飞行时,选择并保持一个方向,若存在未探索边界再返回继续搜寻,结果会比以广度优先搜索的方式来回搜索更快,故本文基于八叉树建图,针对边界驱动方法探索效率低以及探索不充分的问题,提出复合边界点方法,再从众多边界点中利用评估函数选择信息增益最大与偏航角度最小的点作为边界导引点,使无人机在快速飞行的同时能最大限度地降低地图熵,实验结果表明,本文方法具有一定的可行性。

1 构建八叉树地图

目前使用较多的地图主要是点云地图,但点云地图有难以用于导航、地图占用空间大、不方便处理重叠地图等缺陷。八叉树[14]是描述三维空间坐标场景中常用的一种数据结构,其优点是可建模任意环境、随时更新、可动态扩展地图。经过优缺点比较,本文使用八叉树生成的3D占用栅格地图描述环境V,并用从RGBD相机收集的数据来提供点云以更新地图。在八叉树中,3D占用栅格地图中每个体素由其质心表示,先验概率为x=0.5,体素的状态分为空闲Vfree<0.5,占用Vocc>0.5,未知Vunk=0.5

V=Vfree∪Vocc∪Vunk

(1)

无人机飞行时,通常会把地图信息转换成占用概率以供无人机导航,本文将占用概率值小于0.5的体素视为空闲,将占用概率值等于0.5的体素视为未知,将占用概率值大于0.5的体素视为障碍。一个栅格的状态数值越大表示它越可能是占据状态,越小表示它越可能是空闲状态,状态值为初始值则表示它是未知状态,由这3种体素状态定义了整个空间,使无人机能够准确感知环境信息。

2 基于局部环境类型的复合型边界点提取方法

2.1 室内局部环境分类与识别

为提高无人机面对复杂未知环境的瞬时反应速度以及降低计算复杂性,本文针对室内局部环境固有特点,把环境和无人机进行了相对位置模式分类,将当前视角环境分为无障碍飞行模式、无人机朝左边墙飞行模式,无人机朝右边墙飞行模式、右边没有障碍物飞行模式、左边没有障碍物飞行模式、多障碍物飞行模式,如图1所示,无人机根据识别的室内局部环境,规划下一个位置要达到的位置点,再发布命令控制无人机飞往该点。

图1 室内局部环境类型图

如图1所示,模式1为无障碍,在机器人的视野内无障碍物时,无人机将朝扇形视野中心点飞行,飞行角度保持不变,设t为当前时刻时间戳,t+1为无人机下一时刻时间戳,则此时

θt+1=θt

(2)

t∈0,1,2,…,n-1,n

当无人机处于模式2和模式3时,无人机有一部分视野被连续障碍物挡住了,此时选择将这一部分视野丢弃,以模式2为例,如图2所示,此时无人机的视野有一部分被墙面挡住(虚线部分),为了防止无人机撞墙,选择将这一份视野丢弃,找寻剩余的视野的中点(绿色长虚线扇形的中点)。

图2 模式2中无人机下一步飞行示意图

求此时无人机偏航角度如下:假定无人机定高飞行zA=zB,通过八叉树中的光线投射方法(CastRay)访问体素占用状态获取其正前向扇形视野与墙面交点A(xA,yA,zA),采用仿真系统无人机模块获取无人机当前坐标B(xB,yB,zB),无人机原始飞行方向与向量AB之间的夹角为φ,可求得其正切角

(3)

此时φ满足

(4)

设无人机下一个飞行方向与当前飞行方向偏航角为θ,无人机的扇形视角大小为φ,可求得θ

(5)

加入偏航角可以使无人机有效避免被墙面挡住视野。

当无人机处于模式4,5时,表示无人机有部分视野被非连续障碍物挡住,当无人机处于模式6时,表示无人机有部分视野被连续障碍物挡住。

图3 右无障碍模式中无人机下一步飞行示意图

以模式4为例,如图3所示。由于此时无人机左边有障碍物,为了避开障碍物安全飞行,选择朝空闲视角更大的右边飞行,设此时无人机右边扇形视角边缘点为O(xO,yO,zO),障碍物与扇形视角右侧交点为Q(xQ,yQ,zQ),无人机自身坐标为B(xB,yB,zB),则在向量BQ的基础上,偏转一定角度α,得到向量BP,再根据点P与点O坐标可求得无人机下一步边界导引点R

(10)

此处加入偏航角α是为了避免无人机与障碍物过于接近导致摩擦碰撞,主动避免可预见的风险。

随着无人机偏离障碍物一段距离,无人机的视野会逐渐增加到原始视野大小,该方法可以有效提高无人机对环境的能见度,实现快速飞行条件下的安全飞行。

2.2 复合边界点

边界点F定义为具有以下特性的点

F={vF∈Vfree:∃neighbour(vF∈Vunk)}

(11)

针对传统边界点选取规则单一的问题,本文边界点分为2类:局部边界点和全局边界点。

理论上的局部边界点往往分布于无人机视野附近的局部区域,其优点是靠近前沿点能获得较大的信息增益,数量少,便于计算最优候选点,缺点是在探索过程中可能出现候选点数量为零的情况。全局边界点基于可通行空间所有的点,所以候选点的数量较大,分布在全局可通行区域内,全局边界点几乎覆盖整个可通行区域,其优点是能为机器人下一步探索的边界导引点提供更多的选择,相比局部型候选点,鲁棒性更强,缺点是数量巨大,在评估最优点时计算量很大,故相较局部型候选点,全局型候选点需要的计算时间更多。

本文定义的局部边界点分布在无人机视野周边,定义的全局边界点是在视野以外且体素状态介于Vunk和Vfree之间的点。由于单一的边界点提取方法无法同时满足自主探索中高效与全面的要求,本文在探索过程中拟结合2种提取方法对边界点进行复合提取。方法流程如图4所示。

图4 复合边界点选择流程图

无人机主动探索可以认为是一种最小化地图熵的行为,而信息增益表示某点对降低地图不确定性贡献的大小,即能够获取环境信息量的大小,定义地图的香农熵为

(12)

式中，i表示地图的各个栅格单元,j表示栅格的占用情况,可以是占据、空闲、未知3种情况,未知栅格p(mi,j)=0.5,熵值越高,地图不确定性越高,两者呈正比例关系,使用互信息I(m,xi)评估对边界点xi的预期信息增益

I(m,xi)=H(m)-H(m|xi)

(13)

式中：H(m)表示当前地图的熵；H(m|xi)表示在边界点xi处预期的熵。

对于本文提出的复合边界点提取方法,本文采用信息增益及旋转角度非线性加权的效益函数对边界点进行评估,评估函数表达式如下

(14)

式中：λ为加权系数；θq为无人机偏航角；F表示边界点；I(m,xi)为边界导引点处的信息增益估计值,使V(q)值最大的点q为边界导引点。

图5 无人机探索边界导引点示意图

如图5所示,假定无人机当前坐标为(xcurr,ycurr,zcurr),且局部点已探索完毕,而右上角区域未被探索,设红色点为边界导引点,边界导引点的坐标为(xatt,yatt,zatt),可求得两点在x方向的距离xL,y方向上的距离yL,从而求出从当前坐标点到边界导引点的正切角

(15)

再根据反正切函数可得出偏航角度

(16)

在选定边界导引点后,可确定一个吸引力势场

Uatt=γd2(xcurr,xatt)]

(17)

式中：γ是引力势场尺度因子；d(xcurr,xatt)为当前位置与边界导引点之间的距离,使全局点对无人机产生一个吸引力,再给定无人机每一步飞行步长,把这些信息发布给无人机,无人机即可飞往边界导引点实现自主探索。

3 仿真实验与结果分析

为了验证所提出方法的可行性,本文在ubuntu16.04环境下的Gazebo7.0中使用RotorS[15]模型进行了仿真实验,使用RViz来查看即时构建地图的过程和生成的地图、探索路径(绿色线),设计了4种常见的室内局部环境,并将提出的方法与文献7方法进行了比较。为了验证方法的有效性,本文提出的方法在文献[7]方法的公寓环境中进行了实验,对探索时间,相同时间内地图覆盖率进行了对比,如图6～7所示。

图6 公寓环境探索实验结果

图7 公寓环境类型下探索时间与覆盖率关系

地图覆盖率:用八叉树中的被占据的体素Vocc与总体素V之比表示探索覆盖率

(18)

仿真参数如表1所示,因为文献[7]中的模型较小,如果速度过快无人机会无法转弯,出现这种情况是由于没有足够大的空间支持转弯,导致无人机在墙角静止不动或试图转弯导致撞墙。

表1 仿真参数表1

表2 文献[7]中公寓环境类型下本文方法与

通过对公寓环境类型的探索平均时间进行加权计算可得,本文方法比文献[7]方法探索时间降低约68.7%。

为了验证本文方法在不同环境、不同参数下的有效性与普适性,本文模拟了4种环境类型来进行验证,在所有的环境类型下,2种方法无人机放置的起始位置相同,仿真参数如表3所示。

表3 仿真参数表2

本文模拟的环境类型1是单房间,该地图大小为15 m×15 m×3 m,本文方法和文献[7]方法探索效果如图8所示。

图8 环境类型1探索实验结果

图9 单房间环境类型下探索时间与覆盖率关系

在单房间环境类型下,本文方法最短探索用时38 s,文献[7]方法最短探索用时2 161 s,这是由于文献[7]方法需要计算RRT树,不断寻求最佳分支,而本文计算量较小,计算时间几乎可以忽略。

图10 环境类型2探索实验结果

本文模拟的环境类型2是单房间加4个柱状障碍物,该地图大小为15 m×15 m×3 m,本文方法和文献[7]方法探索效果图如图10所示。

图11 单房间+障碍物环境类型下探索时间与覆盖率关系

设计此环境类型的目的是验证2种方法是否能避障,结果表明2种方法都可以较好地避开障碍物,由于加入障碍物,导致2种方法探索时间略有增加,本文方法最短探索用时54 s,文献[7]方法最短探索用时2 435 s。

本文模拟的环境类型3是模仿教室设计,分别有前门和后门,由4个房间加上中间的通道组成,该地图大小为32 m×20 m×3 m,4个房间大小均为11 m×10 m×3 m,中间通道为10 m×20 m×3 m,本文方法和文献[7]方法探索效果如图12所示。

图12 环境类型3探索实验结果

图13 多房间+多通道环境类型下探索时间与覆盖率关系

设计此环境类型的目的是验证2种方法能否在多个房间中探索,单纯在一个房间探索比较简单,体现不出无人机的智能性。实验结果表明,本文方法无人机可以探索得比较全面,而文献[7]方法探索的效果不太理想,总是在同一个地方徘徊,探索覆盖率较低且探索时间很长,其最短探索用时为3 035 s,本文方法最短探索用时仅148 s。

本文模拟的环境类型4设计成1扇墙连通2个房间,在墙中间有一处空隙,该地图大小为20 m×20 m×3 m,中间空隙为3 m,空隙左右两边的墙为8.5 m,本文方法和文献[7]方法探索效果如图14所示。

图14 环境类型4探索实验结果

图15 多房间+单通道环境类型下探索时间与覆盖率关系

设计这个环境类型的目的是考证复合边界点的有效性,通过验证无人机能否有效从空隙穿过探索另一个房间,如果不能从空隙穿过,证明无人机只是被局部点引导飞行,反之,若能从空隙穿过,证明确实有全局点吸引无人机飞行。

实验结果表明,本文方法可以有效探索整个空间,且重复探索区域较少,而文献[7]方法无法探索完整个空间,总是在已探索的地方循环探索,且其最短探索用时2 040 s,本文方法最短探索用时仅107 s。

在这4种情况下,每种方法都以相同的初始配置运行10次,当文献[7]方法出现“planner not reachable”时认为探索结束停止计时。4种方法的探索时间对比如表4所示,其中探索平均时间根据对10次探索时间进行加权平均得到。

表4 不同环境类型下本文方法与文献[7]方法探索时间对比

从探索时间和覆盖率的关系可以看出,采用本文的方法无论在小型环境或大型环境中都基本能够探索全部地图,而文献[7]方法只能在小型环境中探索,一旦进入大型环境则会出现在同一区域重复徘徊的情况。此外,由探索效果图可以看出,本文的方法走重复区域不多,探索环境的顺序比较合理,虽然由于环境类型的复杂性,有时会出现重复探索,这是因为该地方有遗留的全局点待探索,但是总体而言,本文方法极大地减少了探索时间,对4种环境类型的探索平均时间分别进行加权计算可得,对于环境类型1,本文方法比文献[7]方法探索时间约降低98.6%;对于环境类型2,本文方法比文献[7]方法探索时间约降低97.9%;对于环境类型3,本文方法比文献[7]方法探索时间约降低95.7%;对于环境类型4,本文方法比文献[7]方法探索时间约降低96.3%,为了保证方法的有效性及合理性,对计算得到的4种环境类型的探索时间下降率,进行加权计算,得到本文方法比文献[7]方法在4种环境类型下的平均探索时间约降低97.1%。文献[7]方法存在同一区域重复探索的现象,故其探索时间大大增加,且由于文献[7]方法需要在线计算RRT树以找最佳分支,故其计算时间也较长,导致其探索总时间与本文方法相比高出不少。与文献[7]方法相比,本文提出的方法在同样的时间探索覆盖率更高,探索时间更短,故本文提出的方法具有一定的优势。

4 结 论

本文结合了边界驱动方法和复合边界点提取方法对未知环境进行探索,在此基础上用八叉树建立环境地图,使无人机在未知环境下具有自主探索和构建室内环境的能力,与文献[7]方法相比探索同样面积所需时间更少,探索范围更全,路径更为平滑,并迁移在不同参数、不同环境类型下进行了验证,结果表明本文方法具有一定可行性与普适性。