一种基于局部视界拓扑优化的无人机自适应自主探索方法

本发明涉及无人机自主环境感知与机器人路径规划，具体为一种基于局部视界拓扑优化的无人机自适应自主探索方法。

背景技术：

1、小型无人机搭载深度传感设备可在无gps信号的封闭未知环境完成自主空间探索，广泛应用于灾后废墟搜救、地下矿井巡检、室内工业设备排查等作业场景。无人机自主探索完整流程包含多源传感数据采集、三维体素地图重建、自由空间区域划分、全局访问路径规划、局部观测视点排序、平滑轨迹生成与飞行控制六大模块。设备依靠深度相机、里程计、惯性测量单元采集环境几何与自身位姿信息，通过tsdf体素网格完成环境表征，区分未知、自由、占据三类空间状态，提取自由空间与未知空间交界的前沿簇作为后续探索目标，依靠组合优化算法输出无人机飞行路径，实现环境全覆盖探测。现有主流探索框架在开阔单一空间内能够稳定完成环境遍历，但面对多房间分隔、狭长走廊、非凸墙体、多岔路口交织的复杂迷宫场景时，整套系统运行稳定性与空间探索效率存在明显局限。

2、现有方案普遍采用全局重建拓扑图的方式维护自由空间连通关系，每次地图感知更新后，系统会清空全部历史空间分区结果，重新执行三维连通分量标记、区域中心点提取、相邻区域连通路径搜索操作。随着无人机持续飞行，环境体素状态频繁发生增量变化，全局重建机制会持续产生高额计算开销，形成周期性算力峰值。同时反复重新聚类会造成区域中心点位置小幅漂移，相邻区域连通关系同步发生变动，全局路径规划输出的访问序列持续震荡，飞行轨迹频繁调整，增加无效往返飞行行为。传统拓扑简化方式仅以单个中心点代表整块自由空间区域，忽略门洞、走廊通道等真实可通行几何结构，中心点之间的连线不代表实际可飞行通道，宏观路径规划输出的访问顺序在落地执行时容易出现绕路、折返问题。

3、传统局部视点排序仅依靠飞行距离、飞行时长、机身偏航角度变化构建代价矩阵，算法天然偏向就近访问前沿目标，在岔路、房间边角位置容易陷入局部贪婪状态，忽略远处未知空间体量更大、信息价值更高的探测区域。全局规划层面存在建模精度与实时求解速度难以兼顾的矛盾，若完整保留所有区域出入口节点参与组合优化，求解问题规模快速膨胀，机载嵌入式设备无法满足实时运算需求；若仅保留区域中心点参与计算，路径可执行性大幅下降，现有技术缺少分层粒度自适应求解机制，难以同时兼顾近处路径精度与远距离规划计算效率。

技术实现思路

1、本发明的目的在于提供一种基于局部视界拓扑优化的无人机自适应自主探索方法，以解决上述背景技术中提出的问题。

2、一种基于局部视界拓扑优化的无人机自适应自主探索方法，包含如下完整执行步骤：

3、s1将深度图像、里程计位姿数据与imu（惯性测量单元）惯性数据进行时间配准，构建tsdf（截断符号距离函数）体素网格并增量求解esdf（欧氏有符号距离场）距离场，划分体素占用状态，存储体素的连通区域标签、截断距离、累积权重和通行距离约束；

4、s2筛选包围盒内状态变更的体素，通过三维连通分量标记提取自由空间连通域并得到zone（拓扑分区）中心点，将当前中心点与历史中心点做阈值匹配，区分zone的三类状态并执行对应拓扑更新操作，剔除面积过小的微小连通域；

5、s3在相邻zone之间构建连通边，采用局部a*、增量边计算或等效无碰撞路径搜索方法求解可通行路径，提取路径中点作为portal（门户）代表点，并将其记录至相邻zone节点的portal集合中；在portal生成过程中，结合路径搜索碰撞约束、自由空间筛选以及esdf/占据地图约束保证连通路径的可通行性；无有效portal记录的zone将自身中心点作为规划节点；

6、s4以无人机位置划定局部视界，把zone分为近区与远区，近区采用portal节点、远区仅保留中心点；portal数量未超限则构建portal-gtsp（梯度时间同步协议）模型并转换为atsp（非对称旅行商问题）模型求解路径，超限则精简portal并退化为中心点规划，输出混合访问序列；

7、s5提取环境前沿簇并计算信息增益，归一化处理后结合飞行距离与姿态代价构建代价矩阵，优先选取高信息前沿视点，剔除违背飞行约束的候选点，求解得到局部视点访问次序；

8、s6结合视点序列与无人机动力学约束构建三次b样条平滑轨迹并优化偏航角曲线，周期性下发飞行控制指令，循环迭代执行s1至s6，直至空间探测覆盖达标。

9、进一步的，步骤s1中tsdf体素更新采用加权迭代裁剪计算，计算公式为：

10、；

11、式中表示第 (i) 个体素在时刻 (t) 的 tsdf 值，表示该体素上一时刻的 tsdf 值，表示历史累计权重，表示当前深度观测对应的更新权重，表示当前帧深度观测得到的体素截断符号距离，表示 tsdf 截断距离，表示将计算结果限制在范围内。

12、进一步的，步骤s5内部前沿信息增益归一化及重构代价矩阵的核心计算式为：

13、

14、

15、式中，代表第个前沿簇归一化信息增益，表示第个前沿簇，表示该前沿簇包含的体素数量，表示第个前沿簇的信息增益，表示当前参与 sop 排序的候选前沿簇中的最大信息增益，为避免分母为零的极小常数，表示由视点飞向候选前沿或候选视点的原始物理代价，可由飞行距离、飞行时间和偏航变化等因素构成；表示经过信息增益折扣后的重构代价；表示当前规划周期的动态贪婪系数；表示目标前沿的归一化信息增益。目标前沿的信息增益越高，越小，其在 sop 排序中越容易被优先访问。

16、进一步的，步骤s2中zone新旧中心点匹配采用欧氏距离判定规则，设定固定匹配距离阈值，当期任意zone中心点遍历上一周期全部历史zone中心点，两点间欧氏距离小于则判定为同一连通区域演化，直接继承历史zone的连通边、portal集合、未知体素统计数据；当期单个zone中心点匹配到多个历史zone中心点时判定为区域合并状态，仅对存在关联的zone连通边做局部重计算，当期中心点无匹配历史节点则判定为新生zone，分配全新拓扑节点并启动周边连通边搜索。

17、进一步的，步骤s3中portal代表点提取流程依附zone连通边同步完成，两个zone建立连通关系时启动局部a*路径搜索，路径遍历完成后取路径坐标序列中间位置作为portal代表点，分别存入两个zone节点内置portal集合，后续全局gtsp规划阶段直接读取portal集合作为簇内备选节点，未生成portal的zone统一使用区域中心点作为兜底节点，portal存储通行通道真实几何位置，弥补仅依靠zone中心点抽象空间带来的路径可达性缺失问题。

18、进一步的，步骤s4依据视界半径划分近区zone与远区zone，近区调用全部portal参与gtsp计算，远区仅使用zone中心点参与atsp计算；设置portal数量阈值，超限时依据portal与无人机距离、所属zone未知体素数量、连通路径代价等指标筛选代表portal，筛选后数量依旧超限，则统一切换为中心点规划，控制优化问题规模；利用noon-bean变换把gtsp模型转换为atsp模型，调用lkh求解器获取zone与portal的遍历次序；该宏观序列只限定遍历先后顺序，飞行轨迹由局部视点规划模块细化，分层规划降低求解维度。

19、进一步的，步骤s5动态贪婪系数随局部前沿最大体素数量自适应调节，动态贪婪系数计算式采用饱和函数形式，式中表示当前规划周期的信息贪婪系数，表示最大贪婪系数，表示当前参与 sop 排序的候选前沿集合中的最大前沿体素数量，表示饱和调节常数。当当前局部候选前沿规模较大时，增大，系统更倾向于优先访问高信息增益区域；当局部前沿规模减小时，自动降低，规划逐渐回归距离优先；

20、步骤s5设置多层物理约束筛选候选视点，候选视点需同时满足esdf距离大于安全通行距离、相邻视点偏航角差值小于最大偏航变化量、前沿区域处于相机视场内；不满足任一约束则将对应转移代价置为无穷大，不再参与sop排序计算，提前剔除无效视点。

21、进一步的，步骤s6采用三次b样条完成轨迹参数化，统一设置固定控制点间距，轨迹优化阶段同步施加最大线速度、最大线加速度、最大偏航角速度、最大偏航角加速度四维动力学约束，位置轨迹与偏航角轨迹分开优化生成平滑曲线，单次轨迹执行过程中若地图更新造成zone拓扑结构大幅变动或前沿簇总量发生显著变化，立即终止当前轨迹并重新执行全局路径规划与局部视点排序，动态响应环境感知带来的空间结构变化。

22、进一步的，步骤s2在三维连通分量标记完成后增加微小连通域过滤机制，连通域包含自由体素总数量低于最小体素阈值时不注册为有效zone节点，同时依据体素网格尺寸、ccl采样步长、最小未知比例系数计算未知连通域过滤阈值，剔除零散未知区域。

23、进一步的，该方法全程不依赖gps信号，仅依靠机载深度相机、里程计与imu实现感知与定位，算法部署在小型四旋翼无人机嵌入式平台，体素地图采用0.1m精细分辨率与0.2m粗分辨率双层结构，观测距离、tsdf截断距离、网格尺寸、采样步长均依据场景与算力自适应调整。

24、与现有技术相比，本发明的有益效果是：本发明引入区域状态机完成增量式拓扑维护，对地图更新后状态发生改变的体素做局部处理，通过新旧区域中心点距离匹配划分扩张、合并、新生三类空间变化状态，保留历史拓扑结构与连通关系，仅对空间合并、新增区域开展局部连通边修复工作，无需每次地图更新后全局重建拓扑图，能够降低系统计算负载，消除周期性算力峰值，同时避免区域中心点漂移引发的路径序列波动，提升复杂环境下拓扑结构长期稳定程度。

25、本发明在相邻自由区域建立连通关系时同步提取可通行通道代表点作为portal节点，将单一中心点抽象区域拓展为中心点搭配多通道portal的复合空间表征结构，全局路径规划阶段可选用portal节点作为区域内部访问候选点，还原空间真实可通行几何特征，缓解非凸房间、狭长走廊场景下路径规划带来的绕路、折返现象，提升全局访问序列与实际飞行环境的匹配程度。

26、本发明设置局部视界划分远近区域执行混合粒度拓扑规划，无人机近距离待访问区域完整保留portal节点参与高精度组合优化，远距离区域仅依靠区域中心点提供宏观访问方向，同时设置portal数量阈值动态调整求解模型，近区portal数量超出阈值时筛选代表性portal或切换至中心点求解，既维持近处路径的物理通行精度，又控制组合优化问题规模，适配机载嵌入式平台实时运算约束。

27、本发明构建信息驱动的局部视点排序逻辑，以前沿簇包含体素总量衡量区域信息增益，依托当前局部前沿整体规模自适应调整信息权重，未知空间集中阶段优先安排高信息区域访问，未知区域体量缩减后逐步切换为距离优先规划逻辑，缓解传统算法局部贪婪带来的探测遗漏问题，兼顾探索初期大范围未知空间搜寻与后期路径收敛优化需求。

28、本发明对候选观测视点施加多层物理约束，提前剔除存在碰撞风险、相机无法观测未知区域、机身朝向不符合运动限制的无效视点，减少局部排序阶段无意义计算量，轨迹生成模块采用三次b样条完成位置与偏航角曲线独立优化，严格匹配无人机动力学边界条件，生成平滑、安全可直接执行的飞行轨迹，地图拓扑结构大幅变动时可即时重启规划流程，动态适配环境感知带来的空间结构变化，在维持完整环境覆盖效果的基础上优化整体探索效率。