[发明专利]一种适用于腿足机器人规划的稠密高度地图构建方法

说明书

本发明公开了一种稠密高度地图构建方法，属于稠密高度地图构建技术领域。该构建方法具体为：本发明使用高斯分布模型估计激光雷达的测量值，并将其映射到局部地图相应的栅格，以进行多帧数据融合，其间根据机器人位姿信息更新地图栅格协方差，并对局部地图进行相应移动，且对动态障碍物进行清除，以构成完整的局部稠密地图。本发明的方法克服了现有稠密地图数据量大，构建计算量大，更新不及时的不足，并且具有构建方便，构建精度高等优点，并且可以完成实时的构建，可以直接用于腿足机器人的导航任务。

技术领域

本发明涉及稠密高度地图构建技术，具体地说，涉及一种适用于腿足机器人规划的稠密高度地图构建方法。

背景技术

移动机器人是一个集环境感知、动态决策与规划、行为控制与执行等多功能于一体的综合系统，不仅广泛运用于工业、农业、医疗、服务等行业中，而且在城市安全、国防和空间探测领域等对人有害且极度危险的场合得到了很好的应用。近年来，腿足机器人因其卓越的复杂地形可通行性受到全世界的关注并成为研究的热点。相比于轮式和履带式机器人，腿足机器人不仅仅拥有前进、后退和转向等相同的运动控制行为，它还能通过控制前足和后足的落脚点位置，实现“跨越”的动作，这种操作能让腿足机器人轻易通过壕沟、低洼地等地形，也能让其攀上台阶等传统轮式机器人不能通过的复杂场景。

针对地面机器人的路径规划任务，现有的地图表示方法主要使用二维栅格地图描述外界环境。通过地图中的二维栅格是否被占用来表示对应地点是否存在障碍物。这种表示方法构建简单，数据检索快速，适用于轮式和履带式机器人的路径规划，但其无法提供环境地形的高度信息以便腿足机器人的落脚点规划。因此，为了实现腿足机器人的高效运动规划，需要一种能够满足腿足机器人导航规划的可描述复杂地形的稠密地图。稠密地图是由距离传感器实时获取的数据根据机器人位姿信息进行累计而形成的地图，它解决了距离传感器检测范围小、分辨率低的问题，从而能够完整地描述场景的表面形状。稠密地图主要被应用于三维重构以及语义地图的研究中，但是由于其数据量大的特性给实时移动机器人系统上的地图存储和处理带来了较大的挑战。

结合目前常用的传感器，如何构建一种高效的地图更新系统实现稠密地图的实时存储和计算，如何将稠密地图运用于常用的路径导航，都是目前稠密建图中需要解决的的难题和挑战。

发明内容

为了克服现有技术的不足，本发明的目的在于提供一种适用于腿足机器人规划的稠密高度地图构建方法，对于实现腿足移动机器人的长期高效稳定作业有着重要意义。

本发明是通过以下技术方案来实现的：一种稠密高度地图构建方法，包括以下步骤：

步骤一：使用激光雷达以10-20Hz的频率获取周边环境的点云信息，所述点云信息映射到局部高度图中，在局部高度图中，高度测量值为高斯概率分布，其近似为其中，p为测量的高度值，δ_p²为方差。在激光雷达坐标系S下，获取激光雷达到地形的单个测量值将其转换为相应的高度值p，具体为：

其中，表示激光雷达坐标系S转化至全局地图坐标系M的旋转矩阵的逆；为在全局地图坐标系M下SM的距离信息；SM为S点到M点的距离信息；P为投影矩阵，取值为[00 1]。

通过高度值p、单个测量值Φ_SM获得激光雷达测量的距离雅可比矩阵J_s和旋转雅可比矩阵J_Φ，获取所述高度值p的方差δ_p²。

其中，∑_s代表了为激光雷达的协方差矩阵，∑_ΦIS代表了为激光雷达关于角度的协方差矩阵。