CN110146085B - 基于建图和快速探索随机树的无人机实时规避重规划方法 - Google Patents

Abstract

本发明属于无人机***技术领域，公开了一种基于建图和快速探索随机树的无人机实时规避重规划方法。在无人机飞行过程中，通过利用机载深度相机传感器构建以无人机为中心的八叉树环境局部地图，局部地图随着无人机的运动实时更新。在局部地图的基础上，采用基于采样的快速探索随机树的路径规划方法，找出无人机避障路径关键位置点，利用均匀贝兹曲线将路径关键位置点线段组成的路径转换成平滑、动态的可飞行轨迹。本发明提出的方法简单，可用于计算资源有限的无人机机载***中实时避障。

Description

基于建图和快速探索随机树的无人机实时规避重规划方法

技术领域

本发明主要涉及到无人机***技术领域，特指一种基于建图和快速探索随机树的无人机实时规避重规划方法。本发明提出的方法简单，可用于计算资源有限的无人机机载***中实时避障。

背景技术

近年来，无人机的高敏捷性、高机动性、易控制等特点，使其在航拍、巡检、物流和救援等应用领域得到了广泛的应用。大多数情况下无人机的任务环境是未知的，或者存在不可预测的障碍。为了满足未知复杂环境下完全自主飞行的需要，实时局部重规划产生平滑、动态可行的轨迹具有重要意义。避免局部冲突的方法主要有两类：第一类是纯反应性方法，它不需要构建环境地图而是直接根据传感器的数据进行规划，这种方法计算速度快，但是不适用于杂乱的环境，而且会严重受到陷入局部极小值的影响；第二类是基于地图的局部避障方法，它利用各种技术，通过由传感器数据或已知的环境信息构建的局部地图，计算出可行的和局部最优的路径，局部地图构建的难点在于建图的快速性。本专利期望通过构建八叉树局部地图，进行实时局部轨迹重规划来生成无碰撞轨迹，使无人机具有在未知复杂环境中安全自主飞行的能力。

发明内容

本发明要解决的技术问题在于：针对无人机自主避障的需求，提出一种基于建图和快速探索随机树的无人机局部轨迹重规划方法。

针对现有技术存在的问题，本发明内容主要包括三个部分：

1、基于三维循环缓冲区的局部地图构建：通过深度距离传感器得到点云数据，利用体素滤波器对点云进行降采样，然后将点云转化为以八叉树形式存储的栅格地图，并将栅格地图存在以三维数组为存储格式的循环缓冲区。

(1)八叉树地图构建

通过深度距离传感器得到点云，利用体素滤波器对点云进行降采样。然后将点云转化为以八叉树形式存储的栅格地图。具体内容为：

用y∈R来表示八叉树地图中节点的占据情况。当不断感知到该节点被占据时，增加y的值；反之，则减小y的值。用x来表示该节点被占据的概率，概率越大则被占据的可能性越高。x和y之间的转换关系如下：

其中x＝0.5表示未确定，通过概率对数转换，实现地图构建和实时更新，动态的对环境中的障碍物进行建模。

(2)基于三维循环缓冲区的局部地图构建

基于已构建的八叉树地图，通过一个以无人机为中心的三维循环缓冲区来构建局部地图。循环缓冲区是大小为N(N＝(2^p))的三维数组。具体内容为：

假设八叉树地图的分辨率为r，将三维空间中的点p(x,y,z)映射到缓冲区中的点_p'(_x'_,y'_,z')^{的映射过程为} _：

假设缓冲区的中心点设为O(o₁,o₂,o₃)，可以通过以下的公式判断点p'是否在缓冲区内：

缓冲区与四旋翼一起移动。设四旋翼的坐标为Q(q₁,q₂,q₃)。缓冲区中心的初始位置为四旋翼的起点。随着四旋翼飞行器的运动，当q_i-o_i＞0时，o_i＝o_i+d。否则，o_i＝o_i-d，d为缓冲区中心的移动步长。

2、基于快速探索随机树的路径关键位置点生成：初始化随机树的第一个节点，然后在每一次循环过程中，在整个状态空间内产生一个任意的随机点，在产生随机点后，遍历随机树中的每一个节点，计算每一个节点与随机点之间的距离，找出距离此随机点最近的节点，在这两个节点的连线方向上扩展产生一个新节点，即生成一个路径关键位置点；新节点的父节点更新，是在新节点附近的规定范围内寻找相邻节点，作为替换新节点的父节点备选，并选择使得路径代价最小的点作为新节点的父节点。具体内容为：

(1)基于快速探索随机树算法的路径关键位置点生成

首先初始化树产生第一个节点x_init，在每一次循环过程中，产生一个随机点x_rand。随机点的生成是任意的，即可以在整个状态空间内。在产生随机点后，遍历随机树中的每一个节点，计算每一个节点与随机点之间的距离，找出距离此随机点最近的节点，记为x_nearest。在x_nearest与x_rand连线方向扩展产生新的节点x_new，新节点即为路径关键位置点。快速探索随机树算法在新节点x_new附近的规定范围r内寻找相邻节点，作为替换x_new父节点的备选。最终选择使得路径代价最小的点最为x_new的父节点。然后，快速探索随机树算法对树进行重新布线使所有节点的路径代价最小。

3、基于均匀贝兹曲线样条的轨迹平滑：基于均匀贝兹曲线样条的轨迹平滑的部分采用五次均匀贝兹曲线样条来来拟合快速探索随机树生成的路径关键位置点，得到平滑的轨迹。具体内容为：

使用均匀贝兹曲线样条来拟合快速探索随机树生成的路径关键位置点，得到平滑的轨迹。k-1次贝兹曲线样条可由如下公式表示：

p_i∈R³是_ti时刻贝兹曲线样条的控制点，B_i,k(t)是基函数。为了使轨迹能够被无人机跟随，轨迹必须连续到位置的四阶导数。因此使用五次均匀贝兹曲线样条来表示轨迹。均匀贝兹曲线样条控制点之间有固定的时间间隔Δt。对于5次贝兹曲线样条，在时间t∈[t_i,t_i+1)，p(t)的值仅取决于6个连续的控制点[p_i,p_i+1,…,p_i+5]。令:

则轨迹可以表示为：

其中M^k是一个大小为k×k的矩阵

令P＝[p_i,p_i+1,…,p_i+5]^T，可以计算轨迹上任意时刻的速度和加速度。

由于平滑后轨迹会偏离初始路径，存在与障碍碰撞的可能。通过在路径中间增加中点的方法来优化轨迹。假设p₁，p₂，p₃为快速探索随机树算法规划的路径关键位置点，m₁为轨迹段p₁p₂的中点，m₂为轨迹段p₂p₃的中点。m₁和m₂是无碰撞的，因为它位于快速探索随机树搜索算法返回的最优分段线性路径上。计算m₁和m₂的终点p'₂，判断p₁p'₂和p'₂p₃是否与障碍发生碰撞。如果没有，则用p'₂取代路径中的p₂。通过增加路径关键位置点的方法来解决贝兹曲线样条轨迹与障碍发生碰撞的问题，必要时可进行多次插值来确保轨迹的安全。

本发明利用上述提出的方法，可以解决未知环境中无人机的自主避障与路径规划问题。通过构建的八叉树局部地图，可以在杂乱的环境中快速的对环境中的障碍进行建模，所需的时间为毫秒级。在局部地图中使用快速探索随机树实时生成路径关键位置点，并使用均匀贝兹曲线样条对位置点进行拟合，得到平滑且动力学可行的无碰撞轨迹，实现局部障碍的实时规避，使无人机具有在未知复杂环境中安全自主飞行的能力。

附图说明

图1为轨迹规划方法结果示意图。

图2为局部地图构建方法结果示意图。

图3为实时局部轨迹重规划方法结果示意图。

具体实施方式

下面结合附图和具体实施方式对本发明作进一步说明。

(1)基于三维循环缓冲区的局部地图构建

步骤1：从深度距离相机获取图像中各点的深度信息，转化为点云。

步骤2：用一个网格大小为0.2m×0.2m×0.2m体素滤波器对点云进行降采样。

步骤3：根据式(1)，将降采样后的点云***分辨率为0.2m³的八叉树地图中。

步骤4：根据式(2)，将八叉树地图中的点映射到一个由长度为64的三维数组组成的循环缓冲区中。

步骤5：将缓冲区的中心点设为无人机的起点，得到一个跟随无人机运动，且实时更新的局部地图，如图1所示。

(2)基于快速探索随机树的路径控制点生成与轨迹平滑

步骤1：首先确定无人机执行任务的起点S和目标点T，构建局部地图，局部地图随着无人机的飞行实时的更新。

步骤2：记录当前时间t，根据式(1)，计算t时刻对应的全局轨迹上的位置点p作为此次局部规划的目标点。

步骤3：基于构建的局部地图，根据式(3)，如果p点位于局部地图内且p点没有被障碍物占据，采用快速探索随机树算法得到从起点S到目标点p的无碰撞路径，即得到了安全的路径点。否则，等待Δt秒后，重新执行步骤(2)。

步骤4：根据式(6)，将路径点作为控制点***到贝兹曲线样条中，拟合控制点得到平滑且动态可行的轨迹，如图2所示。

步骤5：采用增加路径点的方法对轨迹进行优化处理，使得平滑后的轨迹与初始路径更贴近，保证轨迹的安全。

步骤6：根据式(8)，将得到的轨迹和计算出的速度和加速度发送给无人机，使无人机按照指定的速度跟随轨迹。

步骤7：以一个时间间隔Δt重复步骤(2)～步骤(6)的过程，直到无人机到达最终的目标点T，如图3所示。

以上仅是本发明的优选实施方式，本发明的保护范围并不仅局限于上述实施例，凡属于本发明思路下的技术方案均属于本发明的保护范围。应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理前提下的若干改进和润饰，应视为本发明的保护范围。

Claims (3)

1.基于建图和快速探索随机树的无人机实时规避重规划方法，其特征在于，主要包括三个部分：一是局部八叉树地图构建；二是在所述局部八叉树地图的基础上基于快速探索随机树的路径关键位置点生成；三是基于关键位置点利用均匀贝兹曲线样条进行轨迹拟合平滑；所述局部八叉树地图存储在循环缓冲区中；所述循环缓冲区和无人机同时移动，所述循环缓冲区的缓冲区中心以预设步长移动；所述局部八叉树地图以无人机为中心；

在地图基础上基于快速探索随机树的路径关键位置点生成的步骤为：首先需要初始化随机树的第一个节点，节点代表八叉树地图状态空间内的位置点；设定随机树的搜索次数，在每一次搜索的过程中，首先在八叉树地图状态空间内产生一个任意的随机点，在产生随机点后，遍历随机树中的每一个节点，计算每一个节点与随机点之间的距离，找出距离此随机点最近的节点，然后在这两个节点的连线方向上扩展产生新的节点，即得到一个路径关键位置点；

在局部八叉树地图基础上基于快速探索随机树生成路径关键位置点之前，还包括：

记录当前时间t，计算t时刻对应的全局轨迹上的位置点p作为局部规划的目标点；

判断目标点是否位于局部八叉树地图内和是否被障碍物占据；

若是，则在局部八叉树地图上基于快速探索随机树生成路径关键位置点；

通过深度距离传感器得到点云数据，利用体素滤波器对点云进行降采样，然后将点云转化为以八叉树形式存储的栅格地图，并将栅格地图存储在以三维数组为存储格式的循环缓冲区。

2.根据权利要求1所述的基于建图和快速探索随机树的无人机实时规避重规划方法，其特征在于，

所述基于快速探索随机树的路径关键位置点生成包括，在得到一个新节点后，需要对所述快速探索随机树进行重新布线使得路径的代价最小；首先，需要在新节点附近的规定范围内寻找相邻节点，作为替换新节点的父节点备选，然后选择使得路径代价最小的点作为新节点的父节点。

3.根据权利要求1所述的基于建图和快速探索随机树的无人机实时规避重规划方法，其特征在于，

基于均匀贝兹曲线样条的轨迹平滑的部分采用五次均匀贝兹曲线样条来拟合快速探索随机树生成的路径控制点，得到平滑的轨迹。

Images