CN114355981A - 一种四旋翼无人机自主探索建图的方法和系统 - Google Patents

Abstract

本发明公开一种四旋翼无人机自主探索建图的方法和系统，方法包括S1：获取无人机位姿和无人机深度相机提供的深度图像，建立环境地图；S2：在所述环境地图中进行边界搜索，找出各边界对应的最佳视点，使用最佳视点构建拓扑地图，并搜索保存边界视点之间的最短路径；S3：路径规划，采用LKH算法求解边界视点的遍历问题，得到近似最优遍历顺序，在当前子地图内搜索当前位置到第一个遍历视点的可行局部路径；S4：采用三阶均匀B样条曲线，并引入惩罚项来对轨迹进行优化，得到期望飞行轨迹；S5：将所述期望飞行轨迹发送至无人机的飞行控制器，无人机按照所述期望飞行轨迹飞行。本发明通过构建全局和局部拓扑地图提高路径规划效率。

Description

一种四旋翼无人机自主探索建图的方法和系统

技术领域

本发明涉及移动机器人主动同步定位与建图领域，更具体地，涉及一种四旋翼无人机自主探索建图的方法和系统。

背景技术

随着近年来四旋翼无人机相关技术的不断发展，四旋翼无人机在电力线巡检、农药喷洒、灾难搜救等领域得到了广泛的应用。在这些任务中，无人机的自主探索是一个重要的组成部分，尤其是在未知环境下，无人机需要能够实时感知周围环境，构建对应的环境地图，并依据环境地图实现自主导航。

无人机的自主导航和自主探索都包含两个部分，一是自身定位与环境地图构建，二是运动路径规划。二者的不同点在于，自主导航有明确的路径点，无人机只需规划出一条能够遍历所有路径点的路径即可。而自主探索则是不包含明确的路径点，无人机需要根据已知环境来探索未知区域，需要优化行走的路径，以最小的代价完成未知环境的探索。

近年来，许多探索算法被提出，并运行在各种移动机器人平台上，然而其中的许多算法的探索效率较为低下，对于复杂、大场景的环境适应性不佳。

例如NBVP[1]算法采用贪心策略，只探索即时收益最大的区域或者距离最近的区域，忽略了全局最优性，导致整体的探索效率较低。此外为了保证探索的安全性，运动规划部分较为保守，难以发挥四旋翼飞行器动态性能高的优势。并且算法存在计算复杂度高，当高速飞行，或者周围环境较为复杂时难以及时响应。

[2]中提出的快速探索算法选择优先探索使自身速度变化最小的区域，也就是尽量沿直线飞行，减少转弯或者减速带来的时间损耗，虽然提高了平均探索速度，但是当环境较为复杂的情况下，容易在同一个区域内往复飞行，实际的探索效率并不高。

[3]中提出的算法虽然采用增量式的分层探索规划算法，考虑到了全局探索的最优性问题，并将路径规划分层为全局的路径点遍历顺序问题和局部的运动规划避障飞行问题，提高了计算效率，但是由于缺少合适的全局路径规划器，在面对大场景或者长走廊较多的类迷宫场景下，有可能存在路径规划失败的情况，并且该方法未考虑无人机高速运动下的定位不确定性问题，当环境中存在弱纹理区域时，可能会存在定位丢失，漂移幅度大的问题。

[1]A.Bircher,M.Kamel,K.Alexis,H.Oleynikova,and R.Siegwart,“Recedinghorizon”next-best-view”planner for 3d exploration,”in Proc.of the IEEEIntl.Conf.on Robot.andAutom.(ICRA).IEEE,2016,pp.1462–1468

[2]T.Cieslewski,E.Kaufmann,and D.Scaramuzza,“Rapid explorationwithmulti-rotors:A frontier selection method for high speed flight,”in Proc.ofthe IEEE/RSJ Intl.Conf.on Intell.Robots and Syst.(IROS).IEEE,2017,pp.2135–2142

[3]Zhou B,Zhang Y,Chen X,et al.FUEL:Fast UAV Exploration UsingIncremental Frontier Structure and Hierarchical Planning[J].IEEE Robotics andAutomation Letters,2021,6(2):779-786.

目前基于边界或者基于采样方法的探索策略虽然都能够实现未知环境的探索，但是在面对大场景或者是复杂环境下的鲁棒性不足，主要体现在：

1、当地图变大之后，原有的适用于局部路径规划的算法效率不足，可行路径搜索效率低下，在限定时间内可能找不出可行解。

2、基于贪心策略的搜索算法虽然能够取得即时的最大收益，但是离全局的最优性仍有较大差距，容易出现探索路径重复较大的情况，即无人机在相同区域内多次往复飞行，实际的探索效率低下。

3、自主探索算法依赖于现有的视觉或激光SLAM提供的位姿估计，当环境复杂时可能会影响自定位的准确性，例如探索区域内存在影响视觉特征提取的弱纹理区域，造成所构建地图的变形扭曲、撕裂等等问题。

发明内容

本发明的首要目的是提供一种四旋翼无人机自主探索建图的方法，通过构建全局和局部拓扑地图提高路径规划效率，基于环境先验信息(边界信息、Fisher信息)，引导未知区域探索。

本发明的进一步目的是提供一种四旋翼无人机自主探索建图的系统

为解决上述技术问题，本发明的技术方案如下：

一种四旋翼无人机自主探索建图的方法，包括以下步骤：

S1：获取无人机位姿和无人机深度相机提供的深度图像，建立环境地图；

S2：在所述环境地图中进行边界搜索，找出各边界对应的最佳视点，利用找到的所有最佳视点构建局部拓扑地图，并保存视点之间的可行路径；

S3：路径规划，采用LKH算法求解边界视点的遍历问题，得到近似最优遍历顺序，在当前子地图内搜索当前位置到第一个遍历视点的可行局部路径；

S4：采用三阶均匀B样条曲线，并引入惩罚项来对轨迹进行优化，得到期望飞行轨迹；

S5：将所述期望飞行轨迹发送至无人机的飞行控制器，无人机按照所述期望飞行轨迹飞行。

优选地，所述步骤S1具体为：

利用无人机深度相机获取的双目RGB图像作为输入，使用同步定位与建图算法计算得到无人机当前的位姿信息；

通过无人机深度相机的深度图像获取点云数据，并根据无人机当前的位姿信息将获取的点云数据转换到统一的坐标系下，对点云数据进行体素化滤波，得到当前环境地图；

将当前环境地图以Am*Am*B m的大小，划分成若干个子地图，对于每一个子地图，创建对应的描述信息块，所述描述信息块包括八个顶点坐标、子地图中心坐标、当前子地图与周围六个子地图的连通情况和子地图探索情况，所述连通情况分为未知，可直达和不可直达，探索情况分为未探索、探索未完成和探索已完成。

优选地，所述步骤S2具体为：

无人机在子地图中搜索边界，所述边界定义为当前已知区域与未知区域的交界处，将搜索到的边界进行处理，对于过大的边界进行分割，去每个边界的几何中心点作为该边界的中心，并保存对应边界的体素数量以及位置信息；

以区域内的边界中心为圆心，无人机深度相机的有效距离为最大半径，圆的高度与边界中心的高度一致，取该圆与已知区域交叉部分为采样区域，在区域内进行随机点采样，并取随机点与边界中心的连线作为基线，取与基线成逆时针90度夹角的朝向为机载深度相机朝向角下限，取与基线成顺时针90度夹角的朝向为机载深度相机的朝向角上限，以30度为采样间隔进行均匀采样，用光线投射的方法计算当前采样点的当前采样朝向角所能覆盖的边界体素数量，对随机采样点的所有朝向角的采样结果依据边界体素覆盖范围的从大到小排序，选取拥有最大覆盖范围的采样点作为该边界的最佳视点；

以得到的各个边界的最佳视点为所述局部拓扑地图的节点，其中拓扑节点之间的边权值等于所述节点之间的路径距离；

采用迪杰斯特拉算法计算每一个新增最佳视点到其它已知最佳视点之间的距离，并将距离信息存入边界距离矩阵，得到可行路径，保存所述可行路径。

优选地，在对区域内的边界进行更新时，需要检索在区域内的原有边界是否仍为边界，若当前边界已被探索，则将存储的对应边界信息删除。

优选地，所述步骤S3中将最佳视点的遍历问题作为旅行商问题进行求解，得到近似最佳遍历顺序，具体为：

不同边界的最佳视点之间的飞行代价可以由下式计算得出，其中x_i,x_j为两个不同边界的最佳视点，distance(x_i,x_j)为两个最佳视点之间的距离，可由所述边界距离矩阵直接查询获得，v_max为无人机的最大飞行速度，ξ_i，ξ_j为两个最佳视点的无人机深度相机的朝向角位姿，

为无人机最大的角速度：

将最佳视点的遍历问题当作旅行商问题来求解，采用LKH算法求解近似最佳遍历顺序。

优选地，步骤S3中还需要计算所述局部拓扑地图节点的Fisher信息，计算给定可行路径点不同位姿下的Fisher信息，选取Fisher信息最大的位姿作为对应路径点的期望位姿，为了简化问题，限定位姿的选取只考虑x,y,z坐标以及航向角(yaw角)，即以可行路径点的坐标为中心，Z轴为旋转轴，每30度为间隔，旋转进行采样，将路径点的期望位姿设定为使当前路径点Fisher信息最大的角度。受限于相机的视场范围，为保证飞行的安全性，采样的角度上下限需要在可行路径点两侧有向路径边的夹角范围内。

优选地，所述Fisher信息的计算如下所示：

式中，p(z|x)是条件概率密度函数，x是参数，z是测量值；

假设测量的噪声是均值为0的独立同分布的高斯噪声N(0,δ²)，则上式可以写为：

视觉SLAM算法通过最小化重投影误差来实现对相机位姿的极大似然估计：

式中，n为特征点个数，ξ为相机位姿，Ω为特征点数据之间的协方差，e为对应的重投影误差：

e_i＝u_i-τ(exp(ξ^{^})P_i)

其中u_i为对应特征点的像素坐标，P_i为特征点在基准坐标系下的坐标，τ(·)为相机坐标系到像素坐标系的投影函数，exp(ξ^{^})为相机位姿的李代数指数映射；

假设相机观测的特征点的观测噪声符合零均值的高斯分布，将Fisher信息引入到位姿估计中，可以得到：

其中I_ξ表示对于可观测到的n个特征点，在相机位姿ξ下对应的Fisher信息。

优选地，若当前路径点的所有采样角度的Fisher信息都不满足设定的最小阈值要求，则将该路径点剔除，重新搜索可行路径。

优选地，所述步骤S5具体为：

采用三阶均匀B样条轨迹来对初始拓扑路径进行平滑优化，将拓扑路径上的节点作为B样条轨迹的控制点，将轨迹优化问题转化成一个二次规划问题：

式中，J_s,J_d,J_f分别为轨迹的平滑性惩罚、动力学可行性惩罚以及可观测性惩罚，λ_s,λ_d,λ_f分别为对应的权重系数，受益于B样条曲线的凸包性质，只需要约束各个控制点的二阶和三阶微分项，即速度和加速度，就能约束控制整条曲线以满足要求；

设存在一条B样条曲线，其包含n+1个控制点，分别为{Q₀,Q₁,…,Q_n}，对应的速度，加速度，加加速度如下所示：

平滑性惩罚项J_s可以表示为控制点加速度和加加速度的二范数之和：

动力学可行性惩罚项J_d可以表示为速度，加速度，加加速度是否在给定的范围之内：

p_r表示当前点在x,y,z中某一个方向上的速度或者加速度或者加加速度的标量值；

其中a₁,a₂,b₁,b₂,c₁,c₂的取值需要满足分段函数的二阶连续性，p_m为导数限制，p_j为二次函数和三次函数的交界，λ<1-ε为弹性系数，ε<<1；

J_f为可观测性惩罚项，当Fisher信息I_i大于阈值时，则认为已经能够提供足够的观测信息，不再施加惩罚：

一种四旋翼无人机自主探索建图的系统，包括：

建图模块，所述地图建立模块用于获取无人机位姿和无人机深度相机提供的深度图像，建立环境地图；

边界搜索模块，所述边界搜索模块在所述环境地图中进行边界搜索，找出各边界对应的最佳视点，利用找到的所有最佳视点构建局部拓扑地图，并保存视点之间的可行路径；

路径规划模块，所述路径规划模块采用LKH算法求解边界视点的遍历问题，得到近似最优遍历顺序，在当前子地图内搜索当前位置到第一个遍历视点的可行局部路径；

局部轨迹优化模块，所述轨迹优化模块采用三阶均匀B样条曲线，并引入惩罚项来对轨迹进行优化，得到期望飞行轨迹；

飞行控制模块，所述飞行控制模块将所述期望飞行轨迹发送至无人机的飞行控制器，无人机按照所述期望飞行轨迹飞行。

与现有技术相比，本发明技术方案的有益效果是：

本发明旨在通过构建全局和局部拓扑地图提高路径规划效率，基于环境先验信息(边界信息、Fisher信息)，引导未知区域探索；将探索问题建模成旅行商问题，采用LKH算法找出最优边界遍历顺序，提高探索效率；考虑弱纹理区域影响自定位的可能性，在全局路径规划以及局部的运动规划时引导无人机避开弱纹理区域，引导机载深度相机朝向环境中的显著地标，增加回环，提高自定位的准确性。

附图说明

图1为本发明的方法流程示意图。

图2为本发明的方法框架示意图。

图3为本发明的系统模块示意图。

具体实施方式

附图仅用于示例性说明，不能理解为对本专利的限制；

为了更好说明本实施例，附图某些部件会有省略、放大或缩小，并不代表实际产品的尺寸；

对于本领域技术人员来说，附图中某些公知结构及其说明可能省略是可以理解的。

下面结合附图和实施例对本发明的技术方案做进一步的说明。

实施例1

本实施例提供一种四旋翼无人机自主探索建图的方法，如图1至图2所示，包括以下步骤：

S1：获取无人机位姿和无人机深度相机提供的深度图像，建立环境地图；

S2：在所述环境地图中进行边界搜索，找出各边界对应的最佳视点，利用找到的所有最佳视点构建局部拓扑地图，并保存视点之间的可行路径；

S3：路径规划，采用LKH算法求解边界视点的遍历问题，得到近似最优遍历顺序，在当前子地图内搜索当前位置到第一个遍历视点的可行局部路径；

S4：采用三阶均匀B样条曲线，并引入惩罚项来对轨迹进行优化，得到期望飞行轨迹；

S5：将所述期望飞行轨迹发送至无人机的飞行控制器，无人机按照所述期望飞行轨迹飞行。

所述步骤S1具体为：

利用无人机深度相机获取的双目RGB图像作为输入，使用同步定位与建图算法计算得到无人机当前的位姿信息；

通过无人机深度相机的深度图像获取点云数据，并根据无人机当前的位姿信息将获取的点云数据转换到统一的坐标系下，对点云数据进行体素化滤波，得到当前环境地图；

将当前环境地图以Am*Am*B m的大小，划分成若干个子地图，在本实施例中，子地图的大小为20m*20m*3m，对于每一个子地图，创建对应的描述信息块，所述描述信息块包括八个顶点坐标、子地图中心坐标、当前子地图与周围六个子地图的连通情况和子地图探索情况，所述连通情况分为未知，可直达和不可直达，探索情况分为未探索、探索未完成和探索已完成。

所述步骤S2具体为：

无人机在子地图中搜索边界，所述边界定义为当前已知区域与未知区域的交界处，将搜索到的边界进行处理，对于过大的边界进行分割，去每个边界的几何中心点作为该边界的中心，并保存对应边界的体素数量以及位置信息；

以区域内的边界中心为圆心，无人机深度相机的有效距离为最大半径，圆的高度与边界中心的高度一致，取该圆与已知区域交叉部分为采样区域，在区域内进行随机点采样，并取随机点与边界中心的连线作为基线，取与基线成逆时针90度夹角的朝向为机载深度相机朝向角下限，取与基线成顺时针90度夹角的朝向为机载深度相机的朝向角上限，以30度为采样间隔进行均匀采样，用光线投射的方法计算当前采样点的当前采样朝向角所能覆盖的边界体素数量，对随机采样点的所有朝向角的采样结果依据边界体素覆盖范围的从大到小排序，选取拥有最大覆盖范围的采样点作为该边界的最佳视点；

以得到的各个边界的最佳视点为所述局部拓扑地图的节点，其中拓扑节点之间的边权值等于所述节点之间的路径距离；

采用迪杰斯特拉算法计算每一个新增最佳视点到其它已知最佳视点之间的距离，并将距离信息存入边界距离矩阵，得到可行路径，保存所述可行路径。

在对区域内的边界进行更新时，需要检索在区域内的原有边界是否仍为边界，若当前边界已被探索，则将存储的对应边界信息删除。

所述步骤S3中采用LKH算法求解边界视点的遍历问题，得到近似最优遍历顺序，在当前子地图内搜索当前位置到第一个遍历视点的可行局部路径，具体为：

不同边界的最佳视点之间的飞行代价可以由下式计算得出，其中x_i,x_j为两个不同边界的最佳视点，distance(x_i,x_j)为两个最佳视点之间的距离，可由所述边界距离矩阵直接查询获得，v_max为无人机的最大飞行速度，ξ_i，ξ_j为两个最佳视点的无人机深度相机的朝向角位姿，

为无人机最大的角速度：

将最佳视点的遍历问题当作旅行商问题来求解，采用LKH算法求解近似最佳遍历顺序。

本实施例采用分层路径规划，加速大场景下的路径搜索，包含全局子地图拓扑路径搜索和子地图内局部拓扑路径搜索：如果起始点和目标点在一个子地图内，则直接使用子地图内的拓扑地图进行搜索，若不在一个子地图内，则以子地图为最小搜索单元，进行全局的路径搜索，到达目标子地图后再转为局部拓扑地图搜索。全局子地图拓扑路径搜索算法采用广度优先搜索，局部拓扑地图路径搜索采用迪杰斯特拉算法。

探索遍历顺序优化，环境的探索问题可以看成对未知边界的遍历问题，将遍历问题建模成经典的旅行商问题(TSP)，以边界最佳视点作为路径节点，节点之间的距离作为边的权重，采用LKH算法对探索遍历顺序进行优化，实现对当前边界的一个近似最优遍历。

实施例2

本实施例在实施例1的基础上，引入Fisher信息评估环境路标(landmark)对于无人机自定位算法的效用。引导无人机在全局和局部路径规划时优先在定位质量高的区域内飞行，优化飞行轨迹使得相机指向Fisher信息最为丰富的区域，减少自定位算法的估计误差，具体为：

步骤S3中还需要计算所述局部拓扑地图节点的Fisher信息，计算给定可行路径点不同位姿下的Fisher信息，选取Fisher信息最大的位姿作为对应路径点的期望位姿。

若当前路径点的所有采样角度的Fisher信息都不满足设定的最小阈值要求，则将该路径点剔除，重新搜索可行路径，考虑弱纹理区域影响自定位的可能性，在全局路径规划以及局部的运动规划时引导无人机避开弱纹理区域，引导机载深度相机朝向环境中的显著地标，增加回环，提高自定位的准确性

所述Fisher信息的计算如下所示：

式中，p(z|x)是条件概率密度函数，x是参数，z是测量值；

假设测量的噪声是均值为0的独立同分布的高斯噪声N(0,δ²)，则上式可以写为：

视觉SLAM算法通过最小化重投影误差来实现对相机位姿的极大似然估计：

式中，n为特征点个数，ξ为相机位姿，Ω为特征点数据之间的协方差，e为对应的重投影误差：

e_i＝u_i-τ(exp(ξ^)P_i)

其中u_i为对应特征点的像素坐标，P_i为特征点在基准坐标系下的坐标，τ(·)为相机坐标系到像素坐标系的投影函数，exp(ξ^)为相机位姿的李代数指数映射；

假设相机观测的特征点的观测噪声符合零均值的高斯分布，将Fisher信息引入到位姿估计中，可以得到：

其中I_ξ表示对于可观测到的n个特征点，在相机位姿ξ下对应的Fisher信息。

所述步骤S5具体为：

采用三阶均匀B样条轨迹来对初始拓扑路径进行平滑优化，将拓扑路径上的节点作为B样条轨迹的控制点，将轨迹优化问题转化成一个二次规划问题：

式中，J_s,J_d,J_f分别为轨迹的平滑性惩罚、动力学可行性惩罚以及可观测性惩罚，λ_s,λ_d,λ_f分别为对应的权重系数，受益于B样条曲线的凸包性质，只需要约束各个控制点的二阶和三阶微分项，即速度和加速度，就能约束控制整条曲线以满足要求；

设存在一条B样条曲线，其包含n+1个控制点，分别为{Q₀,Q₁,…,Q_n}，对应的速度，加速度，加加速度如下所示：

平滑性惩罚项J_s可以表示为控制点加速度和加加速度的二范数之和：

动力学可行性惩罚项J_d可以表示为速度，加速度，加加速度是否在给定的范围之内：

p_r表示当前点在x,y,z中某一个方向上的速度或者加速度或者加加速度的标量值；

其中a₁,a₂,b₁,b₂,c₁,c₂的取值需要满足分段函数的二阶连续性，p_m为导数限制，p_j为二次函数和三次函数的交界，λ<1-ε为弹性系数，ε<<1；

J_f为可观测性惩罚项，当Fisher信息I_i大于阈值时，则认为已经能够提供足够的观测信息，不再施加惩罚：

实施例3

本实施例提供一种四旋翼无人机自主探索建图的系统，如图3所示，包括：

建图模块，所述地图建立模块用于获取无人机位姿和无人机深度相机提供的深度图像，建立环境地图；

边界搜索模块，所述边界搜索模块在所述环境地图中进行边界搜索，找出各边界对应的最佳视点，利用找到的所有最佳视点构建局部拓扑地图，并保存视点之间的可行路径；

路径规划模块，所述路径规划模块采用LKH算法求解边界视点的遍历问题，得到近似最优遍历顺序，在当前子地图内搜索当前位置到第一个遍历视点的可行局部路径；

局部轨迹优化模块，所述轨迹优化模块采用三阶均匀B样条曲线，并引入惩罚项来对轨迹进行优化，得到期望飞行轨迹；

飞行控制模块，所述飞行控制模块将所述期望飞行轨迹发送至无人机的飞行控制器，无人机按照所述期望飞行轨迹飞行。

相同或相似的标号对应相同或相似的部件；

附图中描述位置关系的用语仅用于示例性说明，不能理解为对本专利的限制；

显然，本发明的上述实施例仅仅是为清楚地说明本发明所作的举例，而并非是对本发明的实施方式的限定。对于所属领域的普通技术人员来说，在上述说明的基础上还可以做出其它不同形式的变化或变动。这里无需也无法对所有的实施方式予以穷举。凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明权利要求的保护范围之内。

Claims (10)

1.一种四旋翼无人机自主探索建图的方法，其特征在于，包括以下步骤：

S1：获取无人机位姿和无人机深度相机提供的深度图像，建立环境地图；

S2：在所述环境地图中进行边界搜索，找出各边界对应的最佳视点，使用最佳视点构建拓扑地图，并搜索保存边界视点之间的最短路径；

S3：路径规划，采用LKH算法求解边界视点的遍历问题，得到近似最优遍历顺序，在当前子地图内搜索当前位置到第一个遍历视点的可行局部路径；

S4：采用三阶均匀B样条曲线，并引入惩罚项来对轨迹进行优化，得到期望飞行轨迹；

S5：将所述期望飞行轨迹发送至无人机的飞行控制器，无人机按照所述期望飞行轨迹飞行。

2.根据权利要求1所述的四旋翼无人机自主探索建图的方法，其特征在于，所述步骤S1具体为：

利用无人机深度相机获取的双目RGB图像作为输入，使用同步定位与建图算法计算得到无人机当前的位姿信息；

通过无人机深度相机的深度图像获取点云数据，并根据无人机当前的位姿信息将获取的点云数据转换到统一的坐标系下，对点云数据进行体素化滤波，得到当前环境地图；

将当前环境地图以A m*A m*B m的大小，划分成若干个子地图，对于每一个子地图，创建对应的描述信息块，所述描述信息块包括八个顶点坐标、子地图中心坐标、当前子地图与周围六个子地图的连通情况和子地图探索情况，所述连通情况分为未知，可直达和不可直达，探索情况分为未探索、探索未完成和探索已完成。

3.根据权利要求2所述的四旋翼无人机自主探索建图的方法，其特征在于，所述步骤S2具体为：

无人机在子地图中搜索边界，所述边界定义为当前已知区域与未知区域的交界处，将搜索到的边界进行处理，对于过大的边界进行分割，去每个边界的几何中心点作为该边界的中心，并保存对应边界的体素数量以及位置信息；

以区域内的边界中心为圆心，无人机深度相机的有效距离为最大半径，圆的高度与边界中心的高度一致，取该圆与已知区域交叉部分为采样区域，在区域内进行随机点采样，并取随机点与边界中心的连线作为基线，取与基线成逆时针90度夹角的朝向为机载深度相机朝向角下限，取与基线成顺时针90度夹角的朝向为机载深度相机的朝向角上限，以30度为采样间隔进行均匀采样，用光线投射的方法计算当前采样点的当前采样朝向角所能覆盖的边界体素数量，对随机采样点的所有朝向角的采样结果依据边界体素覆盖范围的从大到小排序，选取拥有最大覆盖范围的采样点作为该边界的最佳视点；

以得到的各个边界的最佳视点为所述局部拓扑地图的节点，其中拓扑节点之间的边权值等于所述节点之间的路径距离；

采用迪杰斯特拉算法计算每一个新增最佳视点到其它已知最佳视点之间的距离，并将距离信息存入边界距离矩阵，得到可行路径，保存所述可行路径。

4.根据权利要求3所述的四旋翼无人机自主探索建图的方法，其特征在于，在对区域内的边界进行更新时，需要检索在区域内的原有边界是否仍为边界，若当前边界已被探索，则将存储的对应边界信息删除。

5.根据权利要求4所述的四旋翼无人机自主探索建图的方法，其特征在于，所述步骤S3中将最佳视点的遍历问题作为旅行商问题进行求解，得到近似最佳遍历顺序，具体为：

不同边界的最佳视点之间的飞行代价可以由下式计算得出，其中x_i,x_j为两个不同边界的最佳视点，distance(x_i,x_j)为两个最佳视点之间的距离，可由所述边界距离矩阵直接查询获得，v_max为无人机的最大飞行速度，ξ_i，ξ_j为两个最佳视点的无人机深度相机的朝向角位姿，

为无人机最大的角速度：

将最佳视点的遍历问题当作旅行商问题来求解，采用LKH算法求解近似最佳遍历顺序。

6.根据权利要求5所述的四旋翼无人机自主探索建图的方法，其特征在于，步骤S3中还需要计算所述局部拓扑地图节点的Fisher信息，计算给定可行路径点不同位姿下的Fisher信息，选取Fisher信息最大的位姿作为对应路径点的期望位姿。

7.根据权利要求6所述的四旋翼无人机自主探索建图的方法，其特征在于，所述Fisher信息的计算如下所示：

式中，p(z|x)是条件概率密度函数，x是参数，z是测量值；

假设测量的噪声是均值为0的独立同分布的高斯噪声N(0,δ²)，则上式可以写为：

视觉SLAM算法通过最小化重投影误差来实现对相机位姿的极大似然估计：

式中，n为特征点个数，ξ为相机位姿，Ω为特征点数据之间的协方差，e为对应的重投影误差：

e_i＝u_i-τ(exp(ξ^{^})P_i)

其中u_i为对应特征点的像素坐标，P_i为特征点在基准坐标系下的坐标，τ(·)为相机坐标系到像素坐标系的投影函数，exp(ξ^{^})为相机位姿的李代数指数映射；

假设相机观测的特征点的观测噪声符合零均值的高斯分布，将Fisher信息引入到位姿估计中，可以得到：

其中I_ξ表示对于可观测到的n个特征点，在相机位姿ξ下对应的Fisher信息。

8.根据权利要求7所述的四旋翼无人机自主探索建图的方法，其特征在于，若当前路径点的所有采样角度的Fisher信息都不满足设定的最小阈值要求，则将该路径点剔除，重新搜索可行路径。

9.根据权利要求8所述的四旋翼无人机自主探索建图的方法，其特征在于，所述步骤S5具体为：

采用三阶均匀B样条轨迹来对初始拓扑路径进行平滑优化，将拓扑路径上的节点作为B样条轨迹的控制点，将轨迹优化问题转化成一个二次规划问题：

式中，J_s,J_d,J_f分别为轨迹的平滑性惩罚、动力学可行性惩罚以及可观测性惩罚，λ_s,λ_d,λ_f分别为对应的权重系数，受益于B样条曲线的凸包性质，只需要约束各个控制点的二阶和三阶微分项，即速度和加速度，就能约束控制整条曲线以满足要求；

设存在一条B样条曲线，其包含n+1个控制点，分别为{Q₀,Q₁,…,Q_n}，对应的速度，加速度，加加速度如下所示：

平滑性惩罚项J_s可以表示为控制点加速度和加加速度的二范数之和：

动力学可行性惩罚项J_d可以表示为速度，加速度，加加速度是否在给定的范围之内：

p_r表示当前点在x,y,z中某一个方向上的速度或者加速度或者加加速度的标量值；

其中a₁,a₂,b₁,b₂,c₁,c₂的取值需要满足分段函数的二阶连续性，p_m为导数限制，p_j为二次函数和三次函数的交界，λ<1-ε为弹性系数，ε<<1；

J_f为可观测性惩罚项，当Fisher信息I_i大于阈值时，则认为已经能够提供足够的观测信息，不再施加惩罚：

10.一种四旋翼无人机自主探索建图的系统，其特征在于，包括：

建图模块，所述地图建立模块用于获取无人机位姿和无人机深度相机提供的深度图像，建立环境地图；

边界搜索模块，所述边界搜索模块在所述环境地图中进行边界搜索，找出各边界对应的最佳视点，利用找到的所有最佳视点构建局部拓扑地图，并保存视点之间的可行路径；

路径规划模块，所述路径规划模块采用LKH算法求解边界视点的遍历问题，得到近似最优遍历顺序，在当前子地图内搜索当前位置到第一个遍历视点的可行局部路径；

局部轨迹优化模块，所述轨迹优化模块采用三阶均匀B样条曲线，并引入惩罚项来对轨迹进行优化，得到期望飞行轨迹；

飞行控制模块，所述飞行控制模块将所述期望飞行轨迹发送至无人机的飞行控制器，无人机按照所述期望飞行轨迹飞行。

Images