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

权利要求书

1.一种稠密高度地图构建方法，其特征在于，包括以下步骤：

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

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

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

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

最后通过卡尔曼滤波，将激光雷达最新获取的高度值p以及所述高度值p的方差(p，δ_p²)和局部高度地图对应栅格的估计进行融合，更新获取最新的局部地图高度和局部地图方差δ_h²⁺；

对激光雷达的所有测量值进行上述操作，使所有测量值映射到局部地图对应栅格进行融合，从而构建完整的局部地图。

2.根据权利要求1所述稠密高度地图构建方法，其特征在于，所述局部地图方差δ_h²⁺组成栅格协方差，具体方法为：

所述局部高度图的每个栅格设置k时刻的协方差矩阵

其中，δ_x，min²和δ_y，min²分别表示局部高度图水平面x，y方向不确定性，表示该栅格的高度不确定性；

从时刻k至时刻k+1，机器人从B₁移动至B₂，所述局部高度图的协方差矩阵从变为得到因机器人移动转移而更新的协方差矩阵：

其中，∑_r表示机器人B₁移动至B₂平移上的不确定度，∑_Φ表示B₁移动至B₂旋转变化上的不确定度。