未知环境中无人机自适应边界快速检测算法

唐嘉宁,谢翠娟,赵一帆,李孟霜,彭志祥

(1.云南民族大学 电气信息工程学院, 昆明 650000; 2.云南民族大学 无人自主系统研究院, 昆明 650000)

0 引言

由于无人机具有独特的优势,近年来被广泛应用到救援[1]、测绘[2]、军事[3]等领域。然而,大多数环境在此之前都是部分未知的或完全未知的。自主探索允许机器人在没有任何先验信息的情况下探索和绘制环境,这是全自动机器人的必要能力。

自主探索指执行以前部分未知或完全未知环境建图任务,当所有可访问的空间都被分类为占用或空闲时,探索任务完成。自主探索一般分为3个流程：边界感知检测、路径规划、即时定位与建图(simultaneous localization and mapping,SLAM),3个模块迭代工作,相辅相成,完成整个环境的探索与建图。边界感知检测作为探索的前端,形成边界点以及选出合适的边界点对探索过程来讲尤为重要。边界感知检测又通常分为两步：边界获取与最佳边界点选择。国内外学者提出了多种方法,大致将边界检测分为3类：基于边界的探索、基于采样的探索、基于采样与边界相结合的探索。基于边界的方法主要是通过检测地图中已知区域和未知区域的边界(frontier point,FP),并将FP作为候选目标,利用效用函数对FP进行综合评价,选择最佳FP作为探索目标。Yamauchi等[4]首次提出基于边界探索的重要探索工作,并将此项工作扩展到多机器人[5]。为了有效检测边界,波前边界检测[6]将搜索空间从整个地图减少到局部边界的搜索空间。也有研究致力于减少边界的搜索空间[7-8]。为提高边界处理速度,Batinovic等[9]利用八叉树地图的结构特点,用不同的分辨率表示边界点。Zhou等[10]将全局边界聚类成多个簇,降低计算量,该方法虽然能快速检测到FP,但大量的FP聚类的计算代价无疑是昂贵的。相比之下,Bircher等[11]提出基于采样(next-bext-view-point,NBVP)的方法是在已知空间中采样,并基于一些路径规划算法移动到探索增益最高的姿态。Umari等[12]利用该方法生成全局树和局部树来提高边界检测的效率。Witting等[13]通过维护历史节点提高采样效率。Dharmadhikari等[14]结合无人机飞行动力学和续航能力,提出使用运动原语识别配置空间的路径(GBPlanner)。有学者使用更新快速探索随机树(rapidly-exploring random trees,RRT)节点[15]、前沿滤波[16]、阴影投射算法[17]、动态更新与重定位结合[18]的方法提高探索效率。为了适应地下隧道环境,Dang等[19]提出识别地下环境的关键拓扑特征,提出基于分岔的局部和全局规划结构,实现了周期滑动窗口的快速边界检测,限制了从所有探测区域的自由空间到激光雷达传感范围内的机器人周围局部采样空间,并将该策略扩展到大型洞穴探索[20]和腿式机器人与空中机器人协同的工作中[21]。该方法可以有效地利用采样为无人机提供探索目标,并能减小边界聚类的成本代价,扩展到三维环境较容易。结合八叉树体素结构[22-23],将基于边界和基于采样的前沿点方法结合,提高探索效率。

信息增益被定义为机器人变换一个姿势将获得多少关于环境的新信息,针对不同的目标,提出了不同的信息增益公式,例如快速探索或重建,在最初的前沿探索中,仅考虑距离无人机当前位置最近的边界。NBVP 利用累计未知体素数量选择最佳边界点,尽管取得了进展,但大多数此类工作的一个限制因素与优化的单一目标有关。而且由于环境的复杂性、传感器传回的数据质量、缺乏未探索区域的实体等相关信息,导致整体探索的效率低下,并且增益函数的呈现形式也会变得困难棘手。基于上述问题,将动力学约束[19]和续航能力考虑到增益函数中,对探索的实际应用尤为重要。

本文中提出一种的边界检测器(adaptive detector planner,ADPlanner)。解决在未知环境下,怎样快速完成未知环境的探索,并形成无人机(unmanned aerial vehicle,UAV)可识别的地图。同时,在探索中必须考虑有限续航能力,以及无人机动力学约束的轨迹和姿态估计规划。边界感知检测是UAV自主探索未知环境的关键模块。边界是靠近未知区域的已知区域的集合。因此,ADPlanner利用部署在空中机器人上的激光雷达感知环境和自适应边界框,从而在局部空间采样,通过重要性采样限制重复区域的重采样率,提升采样速率。同时,根据UAV在移动过程中实时建立地图来检测边界,并将评估的最佳边界点发送给路径规划模块,完成大规模复杂地形环境下的高效和稳健的自主探索。

1 问题陈述

本文主要目标是生成自适应采样边界框的边界采样器,解决环境探索运动规划问题。ADPlanner系统流程如图1所示。

定义2(同心松弛)：令Ξ表示无碰撞的连通集合,Nnode⊆Ξ 表示RRT树扩展的节点,κ表示不同节点对边界点选择的影响系数。距离越远,影响系数越小。

问题1(边界检测与探索问题)：给定一个有界未探索空间V, ① 基于激光雷达等传感器得到的占用体素和空闲体素,② 执行时使用{Si}执行完整的环境覆盖,直到不存在任何无碰撞的配置V{Vfree,Vocc}或者Vocc外部可观测表面Socc的任何子集,即Vfree∪Vocc=VVres和Sfree∪Socc=VSres。

2 自适应边界检测

想要实现UAV在大型复杂环境下快速探索的任务,得到安全有效的采样点尤为重要。地下环境复杂多样,隧道宽度有宽有窄,在GBPlanner[19]中采用固定采样框采样并不能很好地适应地下隧道变化起伏的地形,很难确定一个采样框尺寸去适应地下环境,采样框的尺寸太大,在该框中采样点多数为无效点(不能与RRT树连接成边的点均为无效点),采样框的尺寸太小,采样点数量过少,无法形成边界点,直接造成无人机探索任务的失败。因此,在本节中详细介绍自适应采样的边界检测器(oriented bounding box,OBB)[24],所提方法的重点是系统迭代执行探索任务并同时提供高效的建图能力,该过程需要实时决定最佳信息采样行为和相关路径。

2.1 自适应滑动窗口

在得到激光雷达扫描的点云数据local_cloud后,与地图坐标对齐,遍历激光雷达局部点云数据点,利用主成分分析法(principal components analysis,PCA)对数据进行降维分析,得到点云主数据的3个主方向。以无人机当前位置为质心,计算协方差矩阵,求取协方差矩阵的特征值和特征向量,得到采样框的主轴。滑动窗口自适应采样如图2所示,具体算法如下。

算法：自适应采样边界框生成

输入：激光雷达扫描局部点云数据local_cloud

无人机的位姿pose

边界框的最大尺寸boundboxsize_max

输出：自适应采样框的边界值(min_val,max_val)

1.Pi←Local point cloud(pose,local_cloud)

2.Pfilter←Filter(Pi)

3.Fv←get Eigen Vectors(Pfilter)

4.Variance←get Eigen Values(Pfilter)

5.Pcentroid←compute 3D Centroid(Pfilter)

6.δoffset←compute Offset(Pcentroid,pose)

7.δdeviation←cwiseSqrt(Variance)

8.(min_val,max_val)←

compute(δoffset,δdeviation,boundboxsize_max)

9. return (min_val,max_val)

在算法的第1、2行,无人机得到局部(当前时刻激光雷达的扫射范围)激光雷达输入点云数据Pi后,先对点云数据进行过滤,减少点云不均匀密度造成的误差,去除由于视角不同看到的非常接近的点。过滤后得到一系列局部点云的坐标Pfilter=[xi,yi,zi]T。利用协方差矩阵Cov(x,y,z)表示不同坐标之间的相关性：

(1)

E=[E(x),E(y),E(z)]T

(2)

式中：E(x)、E(y)、E(z)是点云坐标x、y、z轴的方差。为了进一步得到主轴,降低3个主轴方向的数据投影到主轴的相关性,将协方差矩阵对角化。利用算法第3—5行对Cov(x,y,z)进行特征向量Fv和特征值Variance的计算,将协方差用特征值组成的对角阵Λ和特征向量组成的矩阵P表示：

Cov(x,y,z)=PΛP-1

(3)

特征向量方向就代表3个主轴的方向,计算出无人机当前自适应采样框的质心Pcentroid：

(4)

利用计算出来的质心Pcentroid和无人机的位姿得到在质心处的偏移量δoffset和标准差δdeviation。由此得到对地下隧道自适应的边界采样框。

2.2 重要性采样

在地下隧道窄道飞行时,采样框的宽度一般大于窄道的宽度,如果此时采样是均匀的,无人机在飞行过程中容易出现重采样,在探索的多次迭代中,累计重采样会降低无人机整体探索效率,增加飞行时间。为了解决这个问题,利用重要性采样提高前后时刻采样边界框非重叠区域的采样率,尽可能引导无人机朝着未知区域探索,提高探索速率。

重要性采样如图3所示,为了限制在前后时刻采样边界框的重叠区域内重复采样率,实现增量前沿检测,根据上一时刻的采样窗口8个角的坐标和下一时刻采样窗口8个角的坐标,分别得到重复采样区域体积Sol、上一时刻采样框体积Ssam1、下一时刻采样框体积Ssam2,进而得到重叠区域的采样点数nol。设ntotal1为上一时刻采样框内的总采样点数,ntotal2为下一时刻采样框内的总采样点数,令前后时刻采样率η相同,从而降低重叠区域的重采样率ηre。

图3 重要性采样

(5)

一般来说,Ssam1和Ssam2是不一样的,因此ntotal1和ntotal2也是不一样的,为了使重复区域的采样率与前后时刻采样框的采样率相同,只有减少重复区域新增采样点个数,增加新增区域采样点数,降低重采样,引导无人机朝新增区域飞行。

2.3 未知环境探索

自适应采样框为未知环境提供了局部视野,是整个探索框架的前端与基础。将整个未知环境地图体素分为3种状态：占用、空闲、未知。在探索过程中,未知体素逐渐被传感器感知,未知体素逐渐被空闲体素和占用体素替代,若探索空间不存在位置体素,代表探索过程结束,即环境建图完成。在这个过程中,并不是每次都能顺利地将预想的未知环境探索完成,总是会出现因采样点不合理造成无人机来回折返,甚至无法探索直接回到起点的情况,但可以利用增益调节系数解决这个问题。

在每次迭代中,路径增益是路径上途径的每个节点增益之和,表示为沿路径上覆盖的体积之和：

(6)

式中：L(nk,nk-1)表示当前点到前沿边界点的欧氏距离;λ表示正常数,衡量了无人机运动距离成本的重要性,λ→0时,边界点选择不受距离影响,λ→∞意味着运动代价太大,λ的值通过实验确定。

在每次迭代中,目标是找到让信息增益最大的路径μbp：

(7)

为了避免在地下隧道等狭窄环境中无人机频繁转向,将路径边界点集合中的偏航角考虑到信息增益中：

(8)

式中：n为路径上的路径点总数,对于n=1的情况,使用UAV当前方向。

在局部规划中考虑无人机当前运动方向和边界点的距离。计算节点的累计增益为：

G=e-(γφk+κD)I(nk)

(9)

式中：D为前沿点到当前点累计根节点的距离,好处在于解决隔着障碍墙的近处点与不隔障碍墙的远处点的选择问题;γ和κ代表方向和距离的影响程度,γ>0,松弛因子κ的大小取决于所选节点在重叠区域内还是重叠区域外,待选节点处于重叠区域外,κ取大于0的常数,在重叠区域内时,κ与Dn成正比,Dn为待选节点到当前节点的欧氏距离。

2.4 复杂度分析

为了进一步理解所提算法的计算复杂度,对边界框进行了详细分析,总的来说,自适应边界框的计算复杂度主要来源于3个方面,即①边界点的选择及自适应边界框的形成,②体素增益的计算,③形成当前点到最佳边界点的路径。这3步最重要的就是前沿边界点的形成。

Ttotal=Tsample+Tgain+Tpath

(10)

在GBPlanner的基础上,采用八叉树(Octomap)地图作为地图框架进行建图,利用自适应滑动窗口进行样本采样,在自适应滑动窗口区域总采样点数为Κ个点,成功采样了N个点,假设在滑动窗口成功采样的平均次数为p,尝试的总次数为p*N,故采样的时间复杂度为：

Tsample=Trand(p*K)+Tnearest(p*K)+

Tstear(p*K)+Tcheckobstacle(p*K)+Tnew(p*N)

(11)

式中：Tsample表示RRG树生长的采样总复杂度时间;Trand、Tnearest、Tstear、Tcheckobstacle、Tnew表示RRG单个树枝生长过程;N为采样成功的点,点的时间复杂度为O。Trand、Tstear、Tcheckobstacle需要执行p*K次,故计算复杂度分别为O(p*K)。Tnew只需要执行p*N次,其余K-N次采样在执行Tcheckobstacle时,不能加入到RRG树中而被丢弃,它的计算复杂度为O(p*N)。Tnearest、Tnew涉及K-D树的搜索与添加,Tnew的计算复杂度为O(N*log2N),在D维K-D树搜索和添加过程的复杂度为O(K1-1/D)和O(KlogK),当D=3,Tnearest的复杂度为O((p*K)2/3),Tnew的复杂度为O(NlogN)。此外,假设采样点数的多少不影响激光雷达处理数据的时间,对于前沿边界点增益的计算和路径形成的复杂度分析,同GBPlanner的一样。激光雷达处理数据的时间可忽略。因此,总体的采样复杂度为：

O(p*K)+O((p*K)2/3)+O(p*K)+

O(p*K)+O(N*logN)≈3p*O(K)+

p2/3*O(K2/3)+O(N*logN)

(12)

根据上述分析,在GBPlanner采用固定尺寸的采样框,每次采样的点数N大于本文方法中的P值,越狭窄的隧道体现的越明显。本文方法在探索地下隧道环境时大大提高了采样时间,减少了迭代次数。环境越空旷,本文算法越等效于GBPlanner,对于计算复杂度,ADPlanner省去了重叠区域的采样点计算。

3 实验

3.1 系统框架

使用自主探索开发环境,无人机飞行参数与GBPlanner的默认设置一致,该算法的运行环境是基于2.5 GHz的i7-11700,运行系统为Ubuntu 20.04,ROS noetic。

为验证本文算法的有效性,将算法应用于模拟地下不规则矿洞以及规则长廊中。UAV飞行参数如表1所示,将本文算法与GBPlanner进行对比,利用探索覆盖率Cov、整个探索过程中无人机飞行的路径长度L、每个滑动窗口的平均采样时间Tsample及探索完未知环境全程算法迭代运行时间T4个方面评估该算法的有效性与快速性。

表1 无人机飞行参数

探索覆盖率表示为：

Cov=Vocc/V

(13)

式中：Vocc表示占用体素个数;V表示待探索环境的总体积。

3.2 地下隧道校验

为了验证提出的ADPlanner的有效性和合理性,将该算法应用于地下隧道不规则环境中,探索效果如图4所示。图4(a)为待探索的环境,图4(b)为探索完成时UAV的建图效果,在图4(c)和图4(d)中,使用GBPlanner出现局部最小值,导致UAV重复探索,降低探索速度。分别用航迹长度L、采样时间Tsample、算法覆盖率、算法总迭代时间T衡量ADPlanner的快速性和鲁棒性,结果如图5所示,GBPlanner和ADPlanner都能将环境探索完成。ADPlanner出现重复探索的几率小,且探索时间缩短。

图4 地下隧道建图

图5 地下隧道环境探索指标曲线

如图6所示,相较GBPlanner,在探索总体性能上,ADPlanner探索路径总长度L缩短18.86%,采样时间Tsample(自适应采样框形成时间与采样点形成时间之和)减少20.2%,完成环境探索任务的总时间T提升了38.38%,极大提高了无人机探索未知环境的效率。

3.3 长廊环境校验

为了验证提出的ADPlanner的有效性和合理性,将该算法应用于长廊环境中,探索效果如图7所示,图7(a)为待探索的环境,图7(b)为探索完成时UAV的建图效果,在图7(c)—(f)中,使用GBPplanner探索出现探索变慢、重复探索等问题,从而降低探索速度。

在长廊环境中,如图8(b)所示,GBPlanner和ADPlanner都能将环境探索完成,但从图8(a)和图8(c)看出,ADPlanner的航迹长度、采样时间明显小于GBPlanner,从而提高整体探索效率,缩短探索时间。

图8 长廊环境探索指标曲线

在探索总体性能上,如图9所示,ADPlanner探索路径总长度L缩短11.24%,采样时间Tsample(自适应采样框形成时间与采样点形成时间之和)减少38.33%,完成环境探索任务的总时间T减少27.38%。

图9 长廊环境环境算法性能

4 结论

针对地下隧道和长廊等狭长多变的复杂未知环境,提出自适应采样的快速边界检测方法。通过自适应滑动采样窗口提高采样的成功率,选择性地在非重叠区域采样,解决重叠区域的重采样问题。利用距离和偏航角评价边界点,在此基础上,通过松弛系数引导无人机向未知环境探索。在2个模拟场景中验证了自适应采样的快速性与适应性。实验结果表明,与GBPlanner相比,自适应边界检测的边界采样时间分别缩短了20.2%～38.33%,路径长度缩短了11.24%～18.86%,算法迭代运行时间缩短了27.38%～38.38%。