一种基于多传感器融合的无人机自主视觉避障方法及系统与流程

本发明涉及无人机避障，具体涉及一种基于多传感器融合的无人机自主视觉避障方法及系统。

背景技术：

1、随着无人机技术在物流、巡检、搜救等领域的广泛应用，其在复杂、动态环境下的自主避障能力成为了决定任务成败与飞行安全的关键。一个典型的无人机自主避障系统通常由环境感知、自身状态估计和路径规划三个核心环节构成。目前，基于多传感器融合的视觉惯性里程计（vio）状态估计技术，结合局部路径规划算法，已成为无人机在gnss信号拒止或弱gnss环境下实现自主导航与避障的主流技术路径。

2、在现有技术中，视觉-惯性紧耦合状态估计器（例如vins-fusion框架）与路径规划器（例如ego-planner框架）的组合已被用于构建无人机避障系统。此类系统通常的工作流程为：首先，由状态估计器通过对双目图像和惯性测量单元（imu）数据的融合，独立计算无人机的自由度位姿；其次，由障碍物检测模块并行处理双目图像，生成障碍物点云信息；最后，路径规划器接收上述位姿和障碍物点云，在构建的局部栅格地图上进行避障路径搜索。在这一技术路径中，状态估计与障碍物信息的处理是彼此分立的，障碍物信息仅作为路径规划的约束条件，而并未对状态估计过程本身产生任何影响。同时，现有方案在构建供路径规划器使用的代价地图时，往往采用简单的二值占据规则或直接投影，缺乏对动态环境和传感器噪声的概率化处理。

3、然而，上述现有技术方案存在明显的技术缺陷。由于状态估计器在优化求解位姿时，完全不考虑环境中的障碍物空间分布，其输出的位姿结果在障碍物密集或距离障碍物过近的区域，容易出现定位精度下降甚至漂移的问题。这种不自知的位姿误差会直接传导至代价地图的构建环节，导致障碍物在全局坐标系下的投影位置不准，进而生成扭曲、不稳定的环境表征。路径规划器基于此类不可靠的输入进行规划，极易导致轨迹平滑度降低、避障失败，甚至在动态环境下完全失效。归根结底，现有技术的架构固化了感知-估计-规划的单向信息流，未能形成闭环反馈，使得系统各环节的性能缺陷会逐级放大，严重制约了整体避障的鲁棒性和成功率。

技术实现思路

1、本发明提供一种基于多传感器融合的无人机自主视觉避障方法及系统，其核心改进在于将障碍物检测信息首次引入到状态估计器的后端联合优化过程中，并以此为基础构建概率更新的实时避障代价地图，旨在从感知源头到规划决策层面，系统性地提升无人机在复杂动态环境下的避障性能。

2、为解决上述技术问题，本发明采用了以下方案：

3、一种基于多传感器融合的无人机自主视觉避障方法，包括以下步骤：

4、s1：获取双目图像及无人机的位姿；

5、s2：基于双目图像进行障碍物检测，实时生成障碍物点云，将障碍物点云作为负向约束，加入状态估计器的后端优化过程中，联合优化无人机位姿，得到远离障碍物区域的无人机位姿；

6、s3：根据优化后无人机的位姿，将障碍物点云从传感器坐标系投影至全局坐标系下的2.5d栅格地图中，并采用更新策略更新2.5d栅格地图中每个栅格的占据概率，生成实时避障代价地图；

7、s4：基于实时避障代价地图和无人机位姿，规划出局部避障路径。

8、本方法将障碍物点云转化为障碍物约束代价项，并加入到状态估计器的后端联合优化过程中，使得求解出的无人机位姿和局部地图点不再是单纯的几何最优解，而是主动远离障碍物区域的空间最优解。这从源头上避免了因状态估计漂移到障碍物上或与之过近而导致的定位精度下降问题。利用经障碍物约束优化后的高精度位姿，将障碍物点云投影至全局坐标系，确保了障碍物在世界坐标系下空间位置的准确性。同时，采用更新策略更新每个栅格的占据概率而非简单的二值占据规则来更新2.5d栅格地图，能够有效融合多帧观测信息，抑制传感器噪声和动态环境引起的误检测，从而输出一个随时间连续、概率稳定的实时避障代价地图。

9、进一步的，s2中，基于双目图像进行障碍物检测，实时生成障碍物点云，具体为：

10、接收去畸变后的无人机双目相机采集的双目图像，采用半全局匹配算法进行立体匹配，生成实时的深度图，再将深度图转换为三维点云，并在该三维点云上执行基于欧式距离的聚类分割算法，将聚类后点数量超过预设阈值的簇标记为障碍物，最终输出定义在双目相机中的左目相机坐标系下的障碍物点云。

11、进一步的，s2中障碍物点云作为负向约束具体为：在状态估计器的代价函数中添加的障碍物距离惩罚项，对于每一个被标识为障碍物的空间点，障碍物距离惩罚项基于当前获取的无人机位姿和局部地图点的预测位置与障碍物距离构建，并通过鲁棒核函数处理，计算得到包含视觉重投影残差、惯性测量单元预积分残差与障碍物距离惩罚项的代价函数的联合最优值，输出具备远离障碍物特性的无人机位姿。

12、进一步的，s2的后端优化具体为：状态估计器的后端维护包含关键帧的滑动窗口，在每次触发后端优化时，状态估计器内部的障碍物约束单元获取最新的障碍物点云。

13、进一步的，s1之前还包括s0：传感器标定，具体为：

14、通过分阶段标定方法，先对双目相机进行内参和双目间外参的标定；

15、再对已标定的双目相机与惯性测量单元进行联合标定，以求解得到惯性测量单元到双目相机的空间变换参数及时间差参数。

16、进一步的，s3中，将障碍物点云从传感器坐标系投影至全局坐标系，具体包括：

17、将障碍物点云从双目相机坐标系，依据已标定的双目间外参转换至无人机的机体坐标系；

18、利用优化后的无人机位姿，将机体坐标系下的障碍物点云再转换至世界坐标系下。

19、进一步的，s3中，采用更新策略更新2.5d栅格地图中每个栅格的占据概率，具体包括：

20、每个栅格维护一个占据概率值；

21、当有障碍物点云投影到某一栅格单元时，依据预设的传感器模型增加栅格单元的占据概率；

22、对于未被障碍物点云投影到的栅格单元，其占据概率随时间按照预设的衰减函数进行衰减，以融合多帧观测信息并适应动态环境。

23、进一步的，s4中局部路径规划器为ego-planner，ego-planner接收实时避障代价地图和优化后的无人机轨迹，并基于实时避障代价地图和优化后的无人机轨迹执行局部避障路径规划，当ego-planner规划的避障路径与避障代价地图中占据概率超过预设安全阈值时，触发局部路径重规划。

24、本发明的另一方面，一种基于多传感器融合的无人机自主视觉避障系统，包括：

25、数据获取模块，用于获取双目图像及无人机的位姿；

26、障碍物检测模块，用于基于双目图像进行障碍物检测，实时生成障碍物点云，将障碍物点云作为负向约束，加入状态估计器的后端优化过程中，联合优化无人机位姿，得到远离障碍物区域的无人机位姿；

27、代价地图构建模块，用于根据优化后无人机的位姿，将障碍物点云从传感器坐标系投影至全局坐标系下的2.5d栅格地图中，并采用更新策略更新2.5d栅格地图中每个栅格的占据概率，生成实时避障代价地图；

28、局部路径规划器，用于基于实时避障代价地图和无人机位姿，规划出局部避障路径。

29、进一步的，还包括传感器标定模块，用于通过分阶段标定方法，先对双目相机进行内参和双目间外参的标定；

30、再对已标定的双目相机与惯性测量单元进行联合标定，以求解得到惯性测量单元到相机的空间变换参数及时间差参数。

31、本发明的有益效果：本发明通过将障碍物信息作为负向约束融入状态估计，纠正了传统架构中因位姿误差导致代价地图扭曲的问题，使得在静态和动态障碍物环境下的避障成功率均可获得显著提升；

32、由于优化后的位姿和概率更新的代价地图更为稳定、准确，路径规划器无需进行频繁的、大幅度的修正，所规划的轨迹更加平滑，加速度方差更低，有效减少了无人机的抖动和不必要的机动，从而降低了飞行能耗；

33、受益于贝叶斯概率更新策略对多帧信息的融合与动态管理能力，系统能够更好地应对障碍物突然出现或移动的场景，不会因单帧检测的瞬间误差而产生过激反应，动态环境下的避障鲁棒性得到质的提升；

34、本发明并非单一模块的优化，而是将障碍物感知深度嵌入状态估计的核心求解过程，其带来的精度红利又无缝传递给规划模块，实现了从感知到控制的全局最优，达成了现有分立式方案所无法企及的性能协同效果。