一种足式探测器非结构环境感知规划方法及系统与流程

本发明属于光电测量领域，涉及一种面向嫦娥七号飞跃器地面验证的非结构环境感知与路径规划方法，尤其适用于地面验证我国未来探月、探火等深空探测工程任务涉及的非结构环境智能感知与导航规划等技术难题。

背景技术：

1、经调研分析可知，国内外无人探测器在环境感知与导航规划方面存在以下问题与不足：

2、(1)无人探测器基于传统立体视觉原理，零基础自主感知复杂崎岖的非结构环境，普遍存在处理信息量大、单帧重建区域小、器载并行计算能力弱等共性问题，严重制约探测器移动探测速度，难以满足大范围、快速移动的工程需求；

3、(2)无人探测器携带的视觉传感器空间环境适应能力相对薄弱，频繁遭遇工作环境的光照、大气(火星环境)、星表地形纹理等因素影响进而导致其性能下降甚至失效，难以兼顾常年阳照区和永久阴影区两种极端环境下的移动探测任务需求；

4、(3)无人探测器在轨移动巡视策略相对保守，普遍采取地面遥操作和自主局部避障相结合且以地面遥操作为主的模式，难以满足“边移动、边巡视、边决策”高效智能自主的工程需求；

5、(4)国内探测器现有的里程估计方法存在积累误差，无法构建精准的全局地图，仅支持探测器单向漫游，无法满足无人探测器自主返航或定期回访的任务需求。

6、为此，迫切需要研究一种面向嫦娥七号飞跃器地面验证的非结构环境感知与路径规划方法，重点突破多源联合标定、建图定位、地形评估(含障碍物检测、斜坡检测、目标点选择等)、路径规划、移动里程估计等核心算法，从根本上解决无人探测器应对多类型极端空间环境实施探测任务所面临的窘境与短板。

技术实现思路

1、本发明解决的技术问题是：克服现有技术的不足，提供在一种非结构环境感知与路径规划方法及系统。

2、本发明解决技术的方案是：一种足式探测器非结构环境感知规划方法，包括

3、s1、在探测器安全着陆稳定后保存探测器着陆后的三轴姿态角静态值；

4、s2、传感器采集探测器移动前进方向的三维地形信息，构建以着陆后的探测器质心为中心圆形区域内的三维地形信息；

5、s3、探测器根据构建的三维地形信息进行可通行性地形评估，根据地形评估结果确定行进方向，自主制定目标点；

6、s4、通过路径寻优规划出当前位置抵达目标点之间的全局安全最优路径；所述全局安全最优路径以探测器质心运动序列形式表示；

7、s5、探测器按照所述全局安全最优路径，遵循先后顺序设置若干个途径位置点，控制探测器依次运动至上述途径位置点；

8、s6、进行移动行走控制：在上述相邻途径位置点的移动过程中，探测器同步控制采集传感器信息，完成多源异构数据融合，实时构建探测器移动至不同位置处的局部三维稠密地图与定位，利用传感器采集的探测器足端周边信息，检测近距离处障碍物信息，动态更新所述局部三维稠密地图，生成移动行走步态规划策略；并同步完成实际移动过程中的里程估计，自主确定探测器当前位置姿态信息；

9、s7、根据探测器当前位置姿态信息，判断探测器是否达到s3制定的目标点，若达到则停止行走；否则结合探测器质心运动序列判断当前行走是否有偏差，没有偏差或者偏差在安全边界内，则返回s6继续执行移动行走控制；若偏差超出安全边界，则返回s4重新进行当前判断位置抵达目标位置之间的全局安全最优路径规划。

10、优选的，所述传感器包括：

11、地形感知装置a：激光雷达1套，负责获取探测器移动前进方向的三维地形信息；

12、地形感知装置b：双目相机1套，负责采集并获取探测器移动前进方向的可见光图像以及三维地形信息；

13、地形感知装置c：地形相机2套，负责采集并检测探测器周边范围内的可见光图像以及障碍物信息；

14、惯性测量单元1套，负责输出探测器当前时刻的三轴姿态角和三轴加速度高频位姿分量。

15、优选的，所述传感器配合使用策略包括：

16、探测器安全着陆稳定后，地形感知装置a或地形感知装置b首次开机获取的三维地形信息直接用于可通行性地形评估；其中，地形感知装置a和地形感知装置b是异构备份；地形感知装置a作为主传感器独立工作，倘若其发生故障无法正常工作，则备份传感器地形感知装置b开机工作，采集左右目图像完成双目立体视觉三维重建，即可得到三维地形信息；

17、探测器在相邻途径位置点的移动过程中，同时控制地形感知装置a或b、惯性测量单元以及地形感知装置c采集多源传感器数据；所述三维地形信息与三轴高频位姿数据一起进行多视点云拼接与配准，用于构建局部三维稠密地图与定位；所述地形感知装置c采集的左右目图像，用于障碍物的立体视觉三维重建，三维重建结果用于近距离障碍物检测。

18、优选的，所述的地形评估包括：

19、读入三维地形信息，根据探测器自身的落足端包络尺寸生成保留高程信息的栅格地图并进行连通域边缘跟踪的平滑滤波填补空洞，计算地图中的每个栅格分别沿前进方向和宽度方向的梯度变化量，按照可通行性评估准则对栅格地图中的障碍物、斜坡特征进行属性标注与膨胀聚类，完成可通行性地形评估。

20、优选的，所述可通行性评估准则包括：

21、不可通行区域：坡面角统计均值＞30°或障碍物外包络尺寸＞20cm×20cm×20cm；

22、低速通行区域：15°＜坡面角统计均值≤30°且障碍物外包络尺寸≤20cm×20cm×20cm；

23、中速通行区域：情况1：8°≤坡面角统计均值≤15°且障碍物外包络尺寸≤20cm×20cm×20cm，或者情况2：坡面角统计均值＜8°且10cm×10cm×10cm＜障碍物外包络尺寸≤20cm×20cm×20cm；

24、高速通行区域：坡面角统计均值≤8°且≤10cm×10cm×10cm。

25、优选的，所述的路径寻优规划包括：

26、读入上述可通行性地形评估结果，将探测器当前位置和目标点位置在栅格地图中进行标定，构建“x-y-angle”2.5维极性搜索空间，定义罚函数确保搜索路径远离障碍物，以目标位置为起点开展八邻域启发式搜索最优路径直至抵达起始位置结束，结合探测器自身运动包络约束，从起始位置至目标位置沿上述搜索路径反向推演安全可通行碰障检测校核，倘若校核正确，则上述搜索结果即可视为全局安全最优路径。

27、一种足式探测器非结构环境感知规划系统，包括：

28、三维重建模块：实时接收安装在探测器上的传感器采集的双目可见光图像信息并进行处理，输出传感器公共观测视场范围内的三维点云数据；所述三维点云数据具体包括三维地形信息和近距离障碍物信息；

29、地形评估模块：读入三维地形信息，结合探测器自身的落足端包络尺寸完成可通行性地形评估，确定行进方向，自主制定目标点，将地形评估结果发送至全局路径规划模块；

30、全局路径规划模块：读入可通行性地形评估结果，结合探测器自身运动包络约束，输出以探测器质心运动序列形式表示的一条全局安全最优路径；

31、畸变校正模块：探测器运动过程中，实时接收传感器采集的标识时间戳的三维地形信息和三轴高频位姿数据，进行多源异构数据融合，生成去畸变后的三维地形信息；

32、局部建图与里程估计模块：读入去畸变后的三维地形信息，实时构建探测器移动至不同位置处的局部三维稠密地图；

33、移动行走控制模块：读入三维重建模块输出的近距离障碍物信息、局部建图与里程估计模块的局部三维稠密地图和里程计信息，将所述近距离障碍物信息动态更新至局部三维稠密地图，据此制定探测器移动行走步态规划策略，下发给探测器底层运动机构执行运动指令，同步反馈局部建图与里程估计模块；

34、局部建图与里程估计模块动态更新所述局部三维稠密地图，并同步完成实际移动过程中的里程估计，自主确定探测器当前位置姿态信息；

35、轨迹跟踪模块：读入局部建图与里程估计模块输出的探测器当前的位置姿态信息，判断探测器是否达到地形评估模块指定的目标点，若达到则停止行走；否则结合探测器质心运动序列判断当前行走是否有偏差，没有偏差或者偏差在安全边界内，则触发移动行走控制模块执行移动行走控制；若偏差超出安全边界，则触发全局路径规划模块重新进行当前判断位置抵达目标位置之间的全局安全最优路径规划。

36、优选的，所述地形评估模块读入三维地形信息，根据探测器自身的落足端包络尺寸生成保留高程信息的栅格地图并进行连通域边缘跟踪的平滑滤波填补空洞，计算地图中的每个栅格分别沿前进方向和宽度方向的梯度变化量，按照可通行性评估准则对栅格地图中的障碍物、斜坡特征进行属性标注与膨胀聚类，完成可通行性地形评估，确定行进方向，自主制定目标点。

37、优选的，所述全局路径规划模块读入上述可通行性地形评估结果，当探测器当前位置和目标点位置在栅格地图中进行标定，构建“x-y-angle”2.5维极性搜索空间，定义罚函数确保搜索路径远离障碍物，以目标位置为起点开展八邻域启发式搜索最优路径直至抵达起始位置结束，结合探测器自身运动包络约束，从起始位置至目标位置沿上述搜索路径反向推演安全可通行碰障检测校核，经校核正确后，则输出以探测器质心运动序列形式表示的一条全局安全最优路径。

38、优选的，所述畸变校正模块在探测器运动过程中，实时接收标识亚毫秒量级时间戳的三维地形信息和三轴高频位姿数据，进行多源异构数据融合，以三轴高频位姿数据为基准进行时间戳对齐，将三维地形信息对应的单帧点云数据统一映射至该帧点云采集的起始时间戳，完成运动畸变校正，生成去畸变后的三维地形信息。

39、优选的，所述局部建图与里程估计模块读入去畸变后的三维地形信息，分别提取corner边缘特征、surface面特征构建关键帧索引，利用pca粗估计相邻点云之间的初始位姿，并计算相邻特征点云中的对应特征的残差和协方差，再利用高斯-牛顿迭代法完成精配准，输出配准后的局部三维稠密地图和里程计信息。

40、优选的，标识时间戳的三维地形信息要求时间戳为亚毫秒量级。

41、本发明与现有技术相比的有益效果是：

42、本发明方法，有效替代以往无人探测器地面遥操作为主、局部自主避障为辅的移动巡视策略，大幅提升足式探测器的探测效能，具备满足“边移动、边巡视、边规划”高效智能自主的工程需求；

43、本发明创新突破了地外天体无人探测器实施移动探测任务无法规避的诸如复杂崎岖的非结构地形环境适应性、极端恶劣光照环境鲁棒性(例如：常年阳照区、永久阴影区等)以及在轨移动探测任务的全自主流程等三大工程难题；

44、本发明提供的环境感知与路径规划方法通过多源传感器互补式感知策略，为飞跃器高效快捷规划可通行路径，具有并行计算效率高、多源融合精度高、地形评估重细节、安全路径搜索快、实时定位自纠偏等特色优势。