CN113741523A - 一种基于边界和采样的混合无人机自主探测方法 - Google Patents

Abstract

本发明提出一种基于边界和采样的混合无人机自主探测方法，包括以下步骤：首先，根据提出的策略进行边界点的检测与提取；其次，基于提取到的边界点，继续进行观测点的采样，通过计算采样边界点处的探测增益选取最佳的观测点，并规划一条基于快速扩展随机树的前往路线；然后，将该观测点作为顶点添加至历史路线图中，并将与历史路线图中原有顶点连成的无碰撞边一并加入路线图中；最后，当没有新的边界点被检测到时，历史路线图规划一条返回最近的待探测边界点的路线，当在整个探测任务中找不到边界点的时候，即视为探测完成；本方法能够保证在计算量受限情况下实现无人机在有界未知环境下的自主探测，并且具有能源消耗小，探测效率高的优点。

Description

一种基于边界和采样的混合无人机自主探测方法

技术领域

本发明属于无人机路径规划技术领域，具体涉及一种基于边界和采样的混合无人机自主探测方法，主要应用于完全未知的三维有界环境下无人机完成自主探索的最优飞行路线规划。

背景技术

近年来，随着科技的发展，无人自主机器人在许多领域均发挥着重要的作用，例如地下矿井，精准农业，灾后救援，和三维重建等等。作为自主机器人中最具代表性的种类之一，无人机凭借着灵活机动、体型较小等特点越来越受到人们的青睐。在无人机众多应用中，对完全未知的三维环境的自主探索不容忽视。精准快速的自主探索能够为人们进一步认识复杂未知的灾后现场、地下矿井等危险环境争取时间，从而减少人员伤亡，在现实生活中有着极其广泛的应用。因此，开展未知的三维有界环境下无人机的自主探测研究具有重要意义。

对于一个给定的有界三维空间

由于其完全未知的特点，在整个探索过程开始之前，V被初始化为未知空间V_unk。本发明要解决的自主探测问题就是确定每一个体素v∈V_unk属于无碰撞空间

还是占据空间

在这个过程中，基于Octomap的三维地图模型M也在同步创建，以实时建模真实的环境。然而在大多数实际场景中，总有一些体素是无法被观测到的，比如空心的空间或者是狭长的走廊等，这些体素均属于不可观测空间

针对可观测空间

有V_obs∪V_res＝V和V_free∪V_occ＝V_obs。

对于在真实场景中飞行的无人飞行器(Micro Aerial Vehicle,MAV)有着旋转和平移6个自由度，然而随着维数的增加，计算复杂度却呈数量级增长。为了简化计算复杂程度，本发明忽略了横滚角和俯仰角的影响，只考虑三个方向上的位置和偏航角，因此无人机的状态x只有四个自由度，即x＝[x,y,z,ψ]^T，其中p＝[x,y,z]^T∈V表示无人机的位置，ψ∈[-π,π)表示偏航角度。

此前，针对未知环境下的自主探测问题主要有两种经典的方法，分别是基于边界的方法和基于采样的方法。对于已建立好的三维地图模型来说，边界点被定义为无碰撞空间V_free和未知空间V_unk的边界，也就是说位于无碰撞空间并且在其6个方向上至少有一个相邻的体素属于未知空间的体素。在配备有深度相机的无人机运动过程中，基于边界的方法通过规划出一条从当前位置到边界点的路径，来不断的探索未知区域，是一种全局规划的思想，但是由于传感器所采集到的数据量过于庞大，需要先过滤，再聚类，以完成数据降维，所以计算资源消耗比较大，对处理器的要求也就比较高。而基于采样的方法是一种局部规划的思想，通过在无人机一定范围内的无碰撞空间中随机采样，通过计算选取增益最高的最佳观测点，继而规划路线前往，虽然一定程度上降低了机器的计算消耗，但是却是一种非启发式的探索策略。由于无人机除了有计算量限制约束之外，还存在着机载能源约束，因此开发一种计算量较小，可以启发式的规划无人机飞行路线的方案具有很大的应用价值。

发明内容

本发明所要解决的技术问题是：针对无人机存在计算量限制、机载能源约束等问题，提供一种基于边界和采样的混合无人机自主探测方法，它是一种计算量小、能源消耗少且能够以更短的飞行距离和飞行时长实现无人机未知环境下自主探测的方法，另外还引入了历史路线图的思想，解决了无人机在计算量和机载能源受限等情况下的精准、快速自主探测问题，提高了其探索效率，尤其是在大规模场景下表现更佳。

本发明的技术解决方案：一种基于边界和采样的混合无人机自主探测方法，其实现步骤如下：

步骤1，无人机实时采集数据，通过与上一次的地图模型对比，检测和提取新的边界点；

边界点被定义为至少有一个方向与未知空间相邻，并且位于无碰撞空间的体素。一般来说，判断体素的相邻有两种方式，分别是6连通(6-connection)和26连通(26-connection)，如图2所示。由于在无人机的飞行过程中会采集到大量满足条件的边界点，所以为了减少数据量，本发明选用6连通的判断方式。

在一次迭代过程中，无人机采集到的大量数据，首先与上一次迭代时的三维地图模型相比，获得位于无碰撞空间中新增的体素集合V_C。然后根据无人机当前状态x和机载深度相机的具体参数，如最大感知距离d_max、垂直视场角α_v和水平视场角α_h等计算每一个体素v∈V_C是否位于无人机的当前视锥范围内，若是则添加到体素集合V_P中。由于地图中任一体素的状态是根据其占据概率确定的，有：

State(v)表示体素v的状态，有无碰撞状态Free、被占据状态Occupied和未知状态Unknown，而p(v)∈[0,1]表示体素的占据概率，通过传感器获取的深度信息叠加得到；

所以这种过滤方法可以为边界点的选取增加一个新的约束，进一步提升获得边界点的准确性。

接下来，对得到的集合V_P进行最后的6-连通判断操作，如果其中的某一个体素v∈V_P在一个或多个方向上与未知空间紧邻，那么则将该体素同时添加至边界点集合V_F和V_NF中，其中V_F指的是全局边界点集合，而V_NF指的是当前迭代过程中所探测到的新边界点的集合。值得一提的是，在每一次迭代正式开始时，集合V_F和V_NF都需要先进行初始化操作，V_F需要判断内部的体素元素是否仍然满足边界点的条件，并删去不满足条件的体素，相比之下V_NF的初始化就简单了许多，直接清空整个集合即可。

步骤2，通过在步骤1获取到的边界点中均匀随机采样得到最佳观测点；

与传统的在距离无人机一定范围内的无碰撞空间中随机采样不同，本发明提出一种新的采样方式，通过在上一步得到的边界点集合中均匀采样，可以得到诸多边界点，进而选择最佳的观测点作为无人机接下来的目标点并前往。

具体来说，为了启发式地选择下一最佳观测点从而更加快速的完成无人机的探测任务，本发明让无人机在第一步得到的集合中均匀采样。但是单纯的在全局边界点集合V_F中采样，很容易使无人机进行不断地往复运动，从而卡在某一区域内。为了解决这一问题，本发明引入了当前迭代中新得到的边界点集合V_NF，通过在其中采样使无人机一直保持向着前方运动直至没有新的边界点被检测到，从而避免的局部的不断往复。本发明预先设置了一个比例系数k，用来平衡无人机在集合V_NF和V_F中的采样次数。对于每k次观测点的采样，有k-1次均来自集合V_NF，一次来自集合V_F。

正如前文所述，本发明考虑的无人机状态是四维的，也就是三个方向的位置和偏航角。由于无人机的运算能力和处理能力较弱，而维度的增加则意味着计算复杂度上以数量级为单位的扩大，因此本发明为了进一步减少运算负载，提升计算效率，特地将最佳偏航角度ψ的选取由随机采样转换为根据位置计算。另外，因为无人机在飞行过程中是平移和旋转同步进行的，而且无人机有一定范围的水平视场角，为了进一步提升计算效率，所以最佳偏航角度的选取可以不需要特别高的精准度。本发明将偏航角的取值范围[-π,π)划分为36个部分，也就是说每两个相邻的方向之间的夹角是10°。

接下来根据采样得到的观测点位置p和相应的最佳偏航角ψ组合得到真正的观测点状态x＝[p,ψ]^Τ，其探测增益由以下公式计算：

gain＝λ·G_visible-(1-λ)·C_dist

其中λ∈[0,1]是预先设置的调整因子，用以平衡可观测深度相机视锥中的未知体素数量G_visible和从当前位置到观测点的距离代价C_dist。为了便于计算，距离的代价函数直接取两点的欧氏距离，由下式计算得到：

其中p_Cx,p_Cy,p_Cz和p_Vx,p_Vy,p_Vz分别指无人机当前位置和经过采样得到的观测点在笛卡尔坐标系三个方向上的坐标。另外，由于具有概率完备性和在无碰撞空间中快速扩展的优点，本发明所涉及的路径规划方法是快速扩展随机树(Rapid-exploration RandomTree，RRT)的。

步骤3、规划一条从当前点前往步骤2中得到观测点的路线，并将无碰撞的路线和观测点分别作为边和顶点加入历史路线图；

在一些相对简单的场景中，无人机只需要不断地向前飞行即可完成探测任务，然而对于一些特殊的场景，例如死胡同或者狭长的走廊等，无人机在完成该区域探索之后需要返回之前存储的未探测的边界点继续开展任务。针对上述需求，为了提升无人机返回待探测边界点的速度，本发明引入了历史路线图(Historical Roadmap，HRM)。HRM在无人机探测阶段和返回待探测边界点阶段均发挥着重要作用。在探测阶段，当无人机处于不断地选择最佳观测点并前往的循环中时，被选中的观测点作为顶点被添加到HRM中，以该观测点为端点，HRM中的其他顶点为另一端点，且与HRM无碰撞的边也被添加到HRM中。

步骤4、当无人机无法在当前节点获取到新的边界点时，根据步骤3中的历史路线图规划一条前往距离最近的、待探测的边界点的路线；

在一次迭代过程中，一旦没有新的边界点被检测到，即

系统自动进入返回待探测边界点阶段，此时无人机就开始根据HRM规划一条返回未探测的、距离最近的观测点。为了简化计算量，本发明选用连续线段而不是优化后的平滑曲线表示规划的路线。当在整个探测任务中找不到边界点的时候，即

则视为探测任务完成。

本发明设计的考虑存在计算量限制、机载能源约束下的基于边界和采样的混合无人机自主探测方法与现有技术相比的优点：

(1)本发明设计了一种基于边界和采样的混合无人机自主探测方法，针对计算量限制、机载能源约束，设计的新型混合探索策略相较于传统的基于边界的方法计算量较小，相较于基于采样的方法，由于启发式思想的存在，其探索时间更少、距离更短、效率更高。

(2)本发明预设了一个比例系数k，用以平衡采样过程中无人机在当前迭代获得的边界点集合V_NF和全局边界点集合V_F中的采样次数。与传统的方法相比，该策略减少了无人机在某一区域的往复运动。

(3)本发明在无人机探索的过程中引入了历史路线图并不断更新，当某一次迭代并未获得新的边界点时规划一条返回待探索区域的最佳路线。与现有的方法相比，该策略返回未探测点更快，进一步提升系统的探索效率。

附图说明

图1为本发明一种基于边界和采样的混合无人机自主探测方法的流程框图；

图2为本发明在边界点检测时不同类型邻接体素的示意图；(a)为6连通(6-connection)边界点，(b)为26连通(26-connection)边界点；

图3为本发明仿真地形二维示意图；

图4为本发明在第一种场景下的仿真结果；

图5为本发明在第一种场景下的仿真结果。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整的描述，显然，所描述的实施例仅为本发明的一部分实施例，而不是全部的实施例，基于本发明中的实施例，本领域的普通技术人员在不付出创造性劳动的前提下所获得的所有其他实施例，都属于本发明的保护范围。

如图1所示，本发明的一种基于边界和采样的混合无人机自主探测方法，该方法是不断迭代进行的，在一次迭代中的步骤为：

首先，根据提出的策略进行边界点的检测与提取；

其次，基于提取到的边界点，继续进行观测点的采样，通过计算采样边界点处的探测增益选取最佳的观测点，并规划一条基于快速扩展随机树的前往路线；所述观测点指的是无人机用于观测空间的状态点，包括位置和偏航，通过不断的采样和计算，选择最大信息增益点作为目标点，也就是下一观测点；

然后，将该观测点作为顶点添加至历史路线图中，并将与历史路线图中原有顶点连成的无碰撞边一并加入路线图中；

最后，当没有新的边界点被检测到时，历史路线图规划一条返回最近的待探测边界点的路线，当在整个探测任务中找不到边界点的时候，即视为探测完成。

整个系统的原理框图如图1所示，具体实施步骤如下：

第一步，进行边界点的检测与提取。边界点被定义为至少有一个方向与未知空间相邻，并且位于无碰撞空间的体素。一般来说，判断体素的相邻有两种方式，分别是6连通(6-connection)和26连通(26-connection)，如图2所示。由于在无人机的飞行过程中会采集到大量满足条件的边界点，所以为了减少数据量，本发明选用6连通的判断方式。

在一次迭代过程中，无人机采集到的大量数据，首先与上一次迭代时的三维地图模型相比，获得位于无碰撞空间中新增的体素集合V_C。然后根据无人机当前状态x和机载深度相机的具体参数，如最大感知距离d_max、垂直视场角α_v和水平视场角α_h等计算每一个体素v∈V_C是否位于无人机的当前视锥范围内，若是则添加到体素集合V_P中。由于地图中任一体素的状态是根据其占据概率确定的，有：

State(v)表示体素v的状态，有无碰撞状态Free、被占据状态Occupied和未知状态Unknown，而p(v)∈[0,1]表示体素的占据概率，通过传感器获取的深度信息叠加得到；

所以这种过滤方法可以为边界点的选取增加一个新的约束，进一步提升获得边界点的准确性。

接下来，对得到的集合V_P进行最后的6-连通判断操作，如果其中的某一个体素v∈V_P在一个或多个方向上与未知空间紧邻，那么则将该体素同时添加至边界点集合V_F和V_NF中，其中V_F指的是全局边界点集合，而V_NF指的是当前迭代过程中所探测到的新边界点的集合。值得一提的是，在每一次迭代正式开始时，集合V_F和V_NF都需要先进行初始化操作，V_F需要判断内部的体素元素是否仍然满足边界点的条件，并删去不满足条件的体素，相比之下V_NF的初始化就简单了许多，直接清空整个集合即可。

第二步，在上一步的基础上，继续进行观测点的采样。与传统的在距离无人机一定范围内的无碰撞空间中随机采样不同，本发明提出一种新的采样方式，通过在上一步得到的边界点集合中均匀采样，可以得到诸多边界点，进而选择最佳的观测点作为无人机接下来的目标点并前往。

具体来说，为了启发式地选择下一最佳观测点从而更加快速的完成无人机的探测任务，本发明让无人机在第一步得到的集合中均匀采样。但是单纯的在全局边界点集合V_F中采样，很容易使无人机进行不断地往复运动，从而卡在某一区域内。为了解决这一问题，本发明引入了当前迭代中新得到的边界点集合V_NF，通过在其中采样使无人机一直保持向着前方运动直至没有新的边界点被检测到，从而避免的局部的不断往复。本发明预先设置了一个比例系数k，用来平衡无人机在集合V_NF和V_F中的采样次数。对于每k次观测点的采样，有k-1次均来自集合V_NF，一次来自集合V_F。

正如前文所述，本发明考虑的无人机状态是四维的，也就是三个方向的位置和偏航角。由于无人机的运算能力和处理能力较弱，而维度的增加则意味着计算复杂度上以数量级为单位的扩大，因此本发明为了进一步减少运算负载，提升计算效率，特地将最佳偏航角度ψ的选取由随机采样转换为根据位置计算。另外，因为无人机在飞行过程中是平移和旋转同步进行的，而且无人机有一定范围的水平视场角，为了进一步提升计算效率，所以最佳偏航角度的选取可以不需要特别高的精准度。本发明将偏航角的取值范围[-π,π)划分为36个部分，也就是说每两个相邻的方向之间的夹角是10°。

接下来根据采样得到的观测点位置p和相应的最佳偏航角ψ组合得到真正的观测点状态x＝[p,ψ]^Τ，其探测增益由以下公式计算：

gain＝λ·G_visible-(1-λ)·C_dist

其中λ∈[0,1]是预先设置的调整因子，用以平衡可观测深度相机视锥中的未知体素数量G_visible和从当前位置到观测点的距离代价C_dist。为了便于计算，距离的代价函数直接取两点的欧氏距离，由下式计算得到：

其中p_Cx,p_Cy,p_Cz和p_Vx,p_Vy,p_Vz分别指无人机当前位置和经过采样得到的观测点在笛卡尔坐标系三个方向上的坐标。另外，由于具有概率完备性和在无碰撞空间中快速扩展的优点，本发明所涉及的路径规划方法是基于快速扩展随机树(Rapid-explorationRandom Tree，RRT)的。

第三步，构建并更新历史路线图。在一些相对简单的场景中，无人机只需要不断地向前飞行即可完成探测任务，然而对于一些特殊的场景，例如死胡同或者狭长的走廊等，无人机在完成该区域探索之后需要返回之前存储的未探测的边界点继续开展任务。针对上述需求，为了提升无人机返回待探测边界点的速度，本发明引入了历史路线图(HistoricalRoadmap，HRM)。HRM在无人机探测阶段和返回待探测边界点阶段均发挥着重要作用。在探测阶段，当无人机处于不断地选择最佳观测点并前往的循环中时，被选中的观测点作为顶点被添加到HRM中，以该观测点为端点，HRM中的其他顶点为另一端点，且与HRM无碰撞的边也被添加到HRM中。

第四步，规划返回待探测边界点路线。在一次迭代过程中，一旦没有新的边界点被检测到，即

系统自动进入返回待探测边界点阶段，此时无人机就开始根据HRM规划一条返回未探测的、距离最近的观测点。为了简化计算量，本发明选用连续线段而不是优化后的平滑曲线表示规划的路线。当在整个探测任务中找不到边界点的时候，即

则视为探测任务完成。

利用Ros/Gazebo进行仿真验证。其仿真地形的二维示意图如图3所示，其中图3(a)场景的大小是20×10×3m，无人机的最大飞行线速度是0.5m/s，最大转向角速度是0.75rad/s，深度传感器最大探测距离是5m，比例系数k是2，水平视场角和垂直视场角分别是90°和60°，地图分别率分别为0.4m和0.2m；图3(b)场景的大小是38×26×3m，无人机的最大飞行线速度是1m/s，最大转向角速度是0.75rad/s，深度传感器最大探测距离是8m，比例系数k是2，水平视场角和垂直视场角分别是90°和60°，地图分别率是0.4m。已探测范围和时间关系的仿真结果分别如图4、图5所示，另外在同样的仿真条件下与经典的NBVP方法进行了对比。

本发明说明书中未作详细描述的内容属于本领域专业技术人员公知的现有技术。尽管为了说明的目的，已描述了本发明的示例性实施方式，但是本领域的技术人员将理解，不脱离所附权利要求中公开的发明的范围和精神的情况下，可以在形式和细节上进行各种修改、添加和替换等的改变，而所有这些改变都应属于本发明所附权利要求的保护范围，并且本发明要求保护的产品各个部门和方法中的各个步骤，可以以任意组合的形式组合在一起。因此，对本发明中所公开的实施方式的描述并非为了限制本发明的范围，而是用于描述本发明。相应地，本发明的范围不受以上实施方式的限制，而是由权利要求或其等同物进行限定。

Claims (5)

1.一种基于边界和采样的混合无人机自主探测方法，其特征在于，包括以下步骤：

步骤(1)、无人机实时采集数据，通过与上一次的地图模型对比，检测和提取新的边界点；

步骤(2)、通过在步骤(1)获取到的边界点中均匀随机采样得到最佳观测点；

步骤(3)、规划一条从当前点前往步骤(2)中得到观测点的路线，并将无碰撞的路线和观测点分别作为边和顶点加入历史路线图；

步骤(4)、当无人机无法在当前节点获取到新的边界点时，根据步骤(3)中的历史路线图规划一条前往距离最近的、待探测的边界点的路线。

2.根据权利要求1所述的基于边界和采样的混合无人机自主探测方法，其特征在于：所述步骤(1)中检测和提取新的边界点，具体包括：

在一次迭代过程中，无人机采集到的大量数据，首先与上一次迭代时的三维地图模型相比，获得位于无碰撞空间中新增的体素集合V_C，然后根据无人机当前状态x和机载深度相机的具体参数，包括：最大感知距离d_max、垂直视场角α_v和水平视场角α_h计算每一个体素v∈V_C是否位于无人机的当前视锥范围内，若是则添加到体素集合V_P中；地图中任一体素的状态是根据其占据概率确定的，有：

其中，State(v)表示体素v的状态，有无碰撞状态Free、被占据状态Occupied和未知状态Unknown，而p(v)∈[0,1]表示体素的占据概率，通过传感器获取的深度信息叠加得到；

接下来，对得到的集合V_P进行最后的6-连通判断操作，如果其中的某一个体素v∈V_P在一个或多个方向上与未知空间紧邻，那么则将该体素同时添加至边界点集合V_F和V_NF中，其中V_F指的是全局边界点集合，而V_NF指的是当前迭代过程中所探测到的新边界点的集合，在每一次迭代正式开始时，集合V_F和V_NF都需要先进行初始化操作，V_F需要判断内部的体素元素是否仍然满足边界点的条件，并删去不满足条件的体素，V_NF的初始化是直接清空整个集合即可。

3.根据权利要求1所述的一种基于边界和采样的混合无人机自主探测方法，其特征在于：所述步骤(2)中，为了启发式地选择下一最佳观测点从而更加快速的完成无人机的探测任务，引入当前迭代中新得到的边界点集合V_NF，通过在其中采样使无人机一直保持向着前方运动直至没有新的边界点被检测到，从而避免的局部的不断往复；预先设置一个比例系数k，用来平衡无人机在集合V_NF和V_F中的采样次数；对于每k次观测点的采样，有k-1次均来自集合V_NF，一次来自集合V_F。

4.根据权利要求3所述的一种基于边界和采样的混合无人机自主探测方法，其特征在于：将偏航角的取值范围[-π,π)划分为36个部分，也就是说每两个相邻的方向之间的夹角是10°，接下来根据采样得到的观测点位置p和相应的最佳偏航角ψ组合得到真正的观测点状态x＝[p,ψ]^Τ，其探测增益由以下公式计算：

gain＝λ·G_visible-(1-λ)·C_dist

其中λ∈[0,1]是预先设置的调整因子，用以平衡可观测深度相机视锥中的未知体素数量G_visible和从当前位置到观测点的距离代价C_dist距离的代价函数直接取两点的欧氏距离，由下式计算得到：

其中p_Cx,p_Cy,p_Cz和p_Vx,p_Vy,p_Vz分别指无人机当前位置和经过采样得到的观测点在笛卡尔坐标系三个方向上的坐标，基于快速扩展随机树的方法进行路径规划。

5.根据权利要求1所述的一种基于边界和采样的混合无人机自主探测方法，其特征在于：所述步骤(3)中，在探测阶段，当无人机处于不断地选择最佳观测点并前往的循环中时，被选中的观测点作为顶点被添加到历史路线图中，以该观测点为端点，HRM中的其他顶点为另一端点，且与HRM无碰撞的边也被添加到HRM中；在一次迭代过程中，一旦没有新的边界点被检测到，即

系统自动进入返回待探测边界点阶段，此时无人机就开始根据HRM规划一条返回未探测的、距离最近的观测点，选用连续线段而不是优化后的平滑曲线表示规划的路线，当在整个探测任务中找不到边界点的时候，即

则视为探测任务完成；所述边界点指的是边界体素，也就是位于free空间中，且至少一个方向与unknown空间相连的体素。

Images