CN111596665A - 一种适用于腿足机器人规划的稠密高度地图构建方法 - Google Patents

Abstract

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

Description

一种适用于腿足机器人规划的稠密高度地图构建方法

技术领域

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

背景技术

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

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

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

发明内容

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

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

步骤一：使用激光雷达以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代表了为激光雷达关于角度的协方差矩阵。

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

进行融合，更新获取最新的局部地图高度

和局部地图方差δ_h ²⁺。

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

进一步地，所述局部地图方差δ_h ²⁺组成栅格协方差，具体方法为：

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

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

表示该栅格的高度不确定性。

从时刻k至时刻k+1，机器人从B₁移动至B₂，所述局部高度图的协方差矩阵从

变为

得到因机器人移动转移而更新的协方差矩阵：

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

进一步地，所述局部地图构建方法还包括：当局部地图移动后，通过机器人的定位估计系统，得到机器人与上一时刻X方向上的位移X_Shift和Y方向上的位移Y_Shift，设定局部地图分辨率为s，并根据机器人的移动信息更新位置偏移值X_StartIndex和Y_StartIndex，获得

局部地图在栅格上的相对位移为X_{index_shift}和Y_{index_shift}：

X_startIndex＝(X_startIndex-round(X_Shift/s)+L_x)％L_x(9)

Y_startIndex＝(Y_startIndex-round(Y_Shift/s)+L_y)％L_y(10)

其中，round()是对小数进行四舍五入获取整数的函数操作，％为求余操作，L_x和L_y分别表示栅格地图的X和Y方向的栅格数量。当需要查询地图(x，y)栅格信息时，通过下列映射查询(x_search，y_search)存储位置的数据内容。

x_search＝(L_x-X_startIndex+x)％L_x (11)

y_search＝(L_y-Y_startIndex+y)％L_y (12)

进一步地，所述局部地图构建方法还包括：

局部地图构建下的点云信息更新直接添加到局部地图中，对于激光雷达的每一个测量值，该测量值到激光雷达在地面投影点的线段上的所有点的地形高度都有高度限制h_limitation：

其中，h_{R_P}为激光雷达到观测点的高度差，L_{R_P}为激光雷达到观测点的水平距离，L_{C_P}为待求高度限制的栅格到观测点的水平距离。若上一时刻局部高度图的高度数据超过相应的高度限制，则代表物体发生移动，相应点的高度信息需要被清空。

相比现有技术，本发明的有益效果在于：本发明的方法使用GPU和多线程并行计算，让整个建图过程可以实时运行。构建了一种2.5D的带有可行域信息的稠密地图，便于腿足机器人的规划。只对判断为障碍物的栅格进行光线追踪，而不是对地图所有区域进行光线追踪，以判断该区域是不是移动障碍物的残影。使用这种策略构建，动态障碍物可以有效地被处理。本发明克服了现有稠密地图数据量大，构建计算量大，更新不及时的不足，并且具有构建方便，构建精度高等优点，并且可以完成实时的构建，可以直接用于腿足机器人的导航任务。

附图说明

图1为本发明稠密高度地图构建方法流程示意图。

具体实施方法

下面，结合附图以及具体实施方式，对本发明的技术方案做进一步地描述：

如图1，本发明提供了一种稠密地图构建方法，包括以下步骤：

步骤一：使用激光雷达以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代表了为激光雷达关于角度的协方差矩阵。

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

进行融合，更新获取最新的局部地图高度

和局部地图方差δ_h ²⁺。

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

所述局部地图方差δ_h ²⁺组成栅格协方差，在更新地图每个栅格协方差的方法中，由于基于M坐标系的局部高度图是相对于传感器/机器人的参考系定义的，因此每当机器人相对于惯性坐标系I运动时，需要根据姿态估计的变化来更新局部地图信息，包括平均高度h和方差值δ_p ²。地图上每个栅格的方差和平均值会根据运动的不确定性以及周围方格的估计值进行更新。

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

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

表示该栅格的高度不确定性。

从时刻k至时刻k+1，机器人从B₁移动至B₂，所述局部高度图的协方差矩阵从

变为

得到因机器人移动转移而更新的协方差矩阵：

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

在完成稠密地图更新后，固定尺寸和像素的稠密栅格地图会伴随着机器人位置的移动而移动。为简化计算量，本方法构建的稠密地图只有沿x和y方向两种方向的平移运动。在这个过程中，小部分稠密图边缘的数据因为可视化区域的变化而被删除，大部分地图区域仅仅更改存储数据的坐标位置但仍保留数据内容。因此，当地图移动时的通常做法是将地图保留数据的存储位置根据机器人的位置变化而变化，但这对大尺寸的栅格地图来说是巨大的计算量。每次地图移动后，地图保留数据之间的相互位置关系从未改变。基于此发现，地图移动后，我们不会改变地图数据的存储位置，而是根据机器人的移动信息来更新位置偏移值X_StartIndex和Y_StartIndex。当需要查询地图不同位置信息时，我们可以使用这两个值来查询所有栅格的数据内容，具体为：

当局部地图移动后，通过机器人的定位估计系统，得到机器人与上一时刻X方向上的位移X_Shift和Y方向上的位移Y_Shift，设定局部地图分辨率为s，并根据机器人的移动信息更新位置偏移值X_StartIndex和Y_StartIndex，获得

局部地图在栅格上的相对位移为X_{index_shift}和Y_{index_shift}：

X_startIndex＝(X_startIndex-round(X_Shift/s)+L_x)％L_x (9)

Y_startIndex＝(Y_startIndex-round(Y_Shift/s)+L_y)％L_y (10)

其中，round()是对小数进行四舍五入获取整数的函数操作，％为求余操作，L_x和L_y分别表示栅格地图的X和Y方向的栅格数量。当需要查询地图(x，y)栅格信息时，通过下列映射查询(x_search，y_search)存储位置的数据内容。

x_search＝(L_x-X_startIndex+x)％L_x (11)

y_search＝(L_y-Y_startIndex+y)％L_y (12)

局部地图构建下的点云信息更新直接添加到局部地图中，而对于新观测中消失的点无法轻易判断出是否是因为物体移动而导致的。因此应对因物体移动需消去原位置点云的问题，主要通过光线追踪的方法来处理，其原理是当前时刻中若地面上的某点可以和激光雷达形成光路，就默认了该点到激光雷达在地面投影点的线段上的每一个点的地形高度都不会阻挡该条光路，即这条线段上的所有点都会有最大的高度限制，即对于激光雷达的每一个测量值，该测量值到激光雷达在地面投影点的线段上的所有点的地形高度都有高度限制h_limitation：

其中，h_{R_P}为激光雷达到观测点的高度差，L_{R_P}为激光雷达到观测点的水平距离，L_{C_P}为待求高度限制的栅格到观测点的水平距离。若上一时刻局部高度图的高度数据超过相应的高度限制，则代表物体发生移动，相应点的高度信息需要被清空。因此应用光线跟踪原理，并根据传感器实时的点云数据，每条光路上栅格的高度限制可以被计算，以实现稠密高度地图上的移动物体移除。

对比本发明方法构建的稠密高度地图，地图每个区域的可行性通过几何结构特征来评估。对于地图的每个栅格，提取其表面法向量(代表局部面的斜率和曲率)和其附近栅格的高度差(代表局部面的粗糙程度)作为局部区域的几何特征。

为了获得栅格P_i处的表面法向量n_i，一个经典的方法是以P_i为原点，结合临近栅格的坐标和高度信息，拟合一个平面S_i＝n_ixx+n_iyy+n_izz。当拟合的平面与所有三维点的距离和最小时，即可求得法向量n_i，具体公式如下：

其中k代表临近区域的栅格数量，Q_i是一个3*K的带有临近K个点的三维坐标信息的矩阵，P_i是待评估栅格二维坐标和高度信息组合的三维向量，n_i＝[n_ix，n_iy，n_iz]是需要求解的法向量。

栅格的高度差可以建立一个以P_(x，y)为中心，大小为N*N的栅格窗口进行计算，这个窗口中栅格的平均高度为

栅格P_(x，y)的高度为

则高度差为：

将单位法向量转换成相应栅格的坡度信息并结合高度差，使用简单而有效的评估公式：

获取每个栅格可通行的分数，其中(x，y)表示栅格所在位置。v_slope(x，y)和v_rough(x，y)为倾斜度和高度差，w_s和w_r为对应的权重，

和

表示该栅格可通行时的对应特征阈值。若最终计算得到可行域分数高于设定可行阈值，则表示该栅格可通行；若该分数低于阈值，表示该部分有障碍物或该地形不适宜通行。

Claims (4)

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

步骤一：使用激光雷达以10-20Hz的频率获取周边环境的点云信息，所述点云信息映射到局部高度图中，在局部高度图中，高度测量值为高斯概率分布，其近似为

其中，p为测量的高度值，δ_p ²为方差。在激光雷达坐标系S下，获取激光雷达到地形的单个测量值

将其转换为相应的高度值p，具体为：

其中，

表示激光雷达坐标系S转化至全局地图坐标系M的旋转矩阵的逆；

为在全局地图坐标系M下SM的距离信息；SM为S点到M点的距离信息；P为投影矩阵，取值为[001]。

通过高度值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₂旋转变化上的不确定度。

3.根据权利要求1所述稠密高度地图构建方法，其特征在于，所述局部地图构建方法还包括：当局部地图移动后，通过机器人的定位估计系统，得到机器人与上一时刻X方向上的位移X_Shift和Y方向上的位移Y_Shift，设定局部地图分辨率为s，并根据机器人的移动信息更新位置偏移值X_StartIndex和Y_StartIndex，获得

局部地图在栅格上的相对位移为X_{index_shift}和Y_{ihdex_shift}：

X_startIndex＝(X_startIndex-round(X_Shift/s)+L_x)％L_x (9)

Y_startIndex＝(Y_startIndex-round(Y_Shift/s)+L_y)％L_y (10)

其中，round()是对小数进行四舍五入获取整数的函数操作，％为求余操作，L_x和L_y分别表示栅格地图的X和Y方向的栅格数量。当需要查询地图(x，y)栅格信息时，通过下列映射查询(x_search，y_search)存储位置的数据内容。

x_search＝(L_x-X_startIndex+x)％L_x (11)

y_search＝(L_y-Y_startIndex+y)％L_y (12)

4.根据权利要求1所述稠密高度地图构建方法，其特征在于，所述局部地图构建方法还包括：

局部地图构建下的点云信息更新直接添加到局部地图中，对于激光雷达的每一个测量值，该测量值到激光雷达在地面投影点的线段上的所有点的地形高度都有高度限制h_limitation：

其中，h_{R_P}为激光雷达到观测点的高度差，L_{R_P}为激光雷达到观测点的水平距离，L_{C_P}为待求高度限制的栅格到观测点的水平距离。若上一时刻局部高度图的高度数据超过相应的高度限制，则代表物体发生移动，相应点的高度信息需要被清空。

Images