CN118329008A - 一种双足机器人导航过程中的局部地形地图构建方法 - Google Patents

Abstract

本发明公开了一种双足机器人导航过程中的局部地形地图构建方法，属于双足机器人建图、自主导航领域。深度相机不断采集地形的点云数据，由感知计算单元进行预处理后与前一帧点云进行配准，获取精准的帧间相对位姿；再根据相对位姿来更新高程图，得到局部地形地图；配准时，使用机器人的定位信息获取两配准点云相对位姿的初值。本发明使用高程图来构造局部地形地图，通过当前帧数据与之前的高程图进行融合，解决了膝盖遮挡带来的当前帧数据无法构造一个信息充分的局部地图的问题。

Description

一种双足机器人导航过程中的局部地形地图构建方法

技术领域

本发明属于双足机器人建图、自主导航领域，具体涉及一种双足机器人导航过程中的局部地形地图构建方法。

背景技术

双足机器人通过能力强，且能像人一样操作，受到广泛关注。近年来，双足机器人稳定控制研究已能初步满足机器人稳定行走需求。自主导航使机器人在无需人工干预的条件下，能够自主地进行运动规划、避障等操作，最终到达目标位置。机器人在导航过程中时，往往会采用屈膝状态行走，因为这可以提高行走的稳定性。然而，屈膝状态会导致机器人脚部前方区域被遮挡，这样仅仅使用当前时刻的传感器数据并不能构建一个信息完整的局部地形地图。

发明内容

针对现有技术中存在不足，本发明提供了一种双足机器人导航过程中的局部地形地图构建方法，解决了当前时刻的地形数据信息不足的问题。

本发明是通过以下技术手段实现上述技术目的的。

一种双足机器人导航过程中的局部地形地图构建方法，其特征在于：

深度相机不断采集地形的点云数据，由感知计算单元进行预处理后与前一帧点云进行配准，获取精准的帧间相对位姿；再根据相对位姿来更新高程图，得到局部地形地图；

配准时，使用机器人的定位信息获取两配准点云相对位姿的初值。

进一步地，所述根据相对位姿来更新高程图，具体是将上一触发深度相机时刻的高程图转到当前时刻下，再将当前时刻的点云数据与转换后的高程图融合。

更进一步地，所述将上一触发深度相机时刻的高程图转到当前时刻下，采用如下转换矩阵：

式中，为当前时刻的机器人body坐标系转到局部地形地图坐标系的变换矩阵；为深度相机坐标系到body坐标系的转换矩阵；为深度相机从上一触发时刻转到当前时刻的转换矩阵，通过点云配准得到；为上一触发深度相机时刻的局部地形地图坐标系到body坐标系的转换矩阵；其中，上一触发深度相机时刻记为时刻1，当前时刻记为时刻2。

更进一步地，对于高程图的转换，具体为：将时刻1高程图内的栅格点转换到时刻2，找到转换后的点在时刻2高程图中对应的栅格，把对应栅格的高度值和协方差使用转换后的值填充。

更进一步地，所述将时刻1高程图内的栅格点转到时刻2，先根据索引遍历时刻1的高程图的每个栅格，得到栅格点的坐标：

p_ij＝(i×d j×d h)^T

式中，i、j为高程图的索引，d为高程图的分辨率，h为栅格的高度；

栅格点的协方差为：

式中，为栅格点在x，y方向的协方差，为栅格点在高度方向的协方差；

根据p_ij和将时刻1高程图内的栅格点转换到时刻2：

式中，p_ij2为时刻2的栅格点坐标，为时刻2的栅格点的协方差。

更进一步地，找到转换后的点在时刻2高程图中对应的栅格，为：

式中，x₂、y₂为p_ij2的x、y坐标，i₂、j₂为p_ij2在时刻2高程图对应栅格的索引，为取整符号。

更进一步地，把对应栅格的高度值和协方差使用转换后的值填充：该栅格的高度值为p_ij2的z坐标，高度值的协方差为第三行第三列的值。

更进一步地，所述再将当前时刻的点云数据与转换后的高程图融合，将时刻2的点云数据转到高程图的局部地形地图坐标系下，将转换后的点加入到时刻2的高程图中。

更进一步地，所述将当前时刻的点云数据转到高程图的局部地形地图坐标系下，采用如下方式：

式中，p₂为时刻2的深度相机坐标系下的点坐标，p'₂为时刻2的高程图坐标系下的点坐标，为点在深度相机坐标系下的协方差，为点在时刻2的高程图坐标系下的协方差。

更进一步地，所述将转换后的点加入到当前时刻的高程图中，具体为：找到点在时刻2的高程图中对应的栅格，如果该栅格没有数据，那么该栅格的高度为p'₂的z坐标，高度的协方差为的第三行第三列的值；如果该栅格有数据，将时刻2的高程图中已有的数据与转换后的数据进行融合：

式中，为时刻2的高程图的栅格原有的高度值，为时刻2的高程图的栅格原有高度值的协方差；p_2z为转换后的点的高度值，即为p'₂的z坐标；为转换后的点的高度值协方差，即为第三行第三列的值，为融合后的高度值，为融合后高度值的协方差。

本发明的有益效果为：

(1)本发明在自主导航过程中，当前时刻的地形数据加入到高程图，并与高程图内已有的数据融合，构建局部地形地图；即使由于膝盖遮挡导致当前时刻的地形数据不包含机器人脚前方的区域，只要之前的时刻采集到该区域的地形数据，就会被高程图记录，解决了膝盖遮挡带来的当前帧数据无法构造一个信息充分的局部地图的问题。

(2)在构造局部地形地图时，本发明通过一个有良好初值的点云配准估计出了精准的两帧点云的相对位姿，为构造一个高精度的地形地图打下基础。

附图说明

图1为本发明所述感知部件安装示意图；

图2为本发明所述深度相机地形感知示意图；

图3为本发明所述局部地形地图构建流程图；

图4(a)为本发明飞点去除前效果图；

图4(b)为本发明飞点去除后效果图；

图5为本发明定位时刻与深度相机采集时刻示意图；

图6为本发明两个时刻机器人的深度相机、body和地图坐标系示意图；

图7(a)为本发明时刻1的高程图；

图7(b)为本发明时刻1转换到时刻2的高程图；

图7(c)为本发明时刻2的高程图。

具体实施方式

下面结合附图以及具体实施例对本发明作进一步的说明，但本发明的保护范围并不限于此。

本发明涉及的感知部件包括激光雷达、深度相机和感知计算单元，如图1所示。感知计算单元用于接收激光雷达和深度相机采集的数据，并进行处理。其中激光雷达水平安装在机器人头部，且其x轴方向与机器人前进方向重合，激光雷达用于机器人定位，即通过激光雷达采集的点云，与现有的环境地图进行配准获取机器人相对于环境地图的位姿。由于激光雷达采集的点云噪声较大，而且稀疏，所以定位效率很高，但精度较低；定位频率即为激光雷达产生点云的频率，一般为10Hz。深度相机安装在机器人的胸部，其视线朝下，用于采集地形数据，但是机器人的膝盖会遮挡深度相机的视野，如图2所示。

为了防止机器人行走误差累积，同时具备适应环境变化的能力，机器人每走一步都会重新构建局部地形地图，并使用该地图进行落脚点规划，确保机器人在下次迈脚时能按照新规划的落脚点行走。这就要求，机器人必须在一个迈步周期内完成局部地形地图构建及落脚点规划，为了给地图构建和落脚点规划留出充足的时间，在每次机器人抬脚时，深度相机捕获当前时刻的地形数据并发送给感知计算单元，构造此刻的局部地形地图，再进行落脚点规划。因此，深度相机采集点云的频率与机器人的迈步频率相同，一般小于1Hz。

本发明描述的局部地形地图构建流程如图3所示，深度相机会不断采集地形的点云数据，由感知计算单元进行预处理后与前一帧点云进行配准，获取精准的帧间相对位姿。配准时，需要使用机器人的定位信息获取两配准点云相对位姿的初值，这样可以提高配准效率；再根据相对位姿来更新高程图，这样即可得到局部地形地图。下面对具体步骤进行详细说明：

(1)点云预处理，包括体素滤波和飞点去除。

体素滤波是一种基于体素格子的降采样算法，主要用于减少深度相机采集的点云数量，同时尽可能保留点云的形状特征，提高配准效率。

飞点可以被认为是物体的阴影，是一些真实物理世界不存在的点，如图2所示。飞点会影响点云的配准精度，甚至会导致配准错误。根据飞点特征，可以使用点的方向与法向量的夹角来滤除飞点，即如果点的法向量和方向满足式(1)，那该点认为是飞点，需要去除：

式中，n_p表示点的法向量；表示点的方向，是一个单位向量；T_f表示设定的阈值。飞点去除前后的效果图参见图4(a)、(b)。

(2)点云配准

点云配准可以获得点云之间精准的相对位姿，但其在配准过程中容易陷入局部极值，这就需要有一个良好的初始值，它不仅能避免配准陷入局部极值，而且还能减少配准迭代次数，提高配准效率。

如图5所示，由于深度相机的点云采集时刻与激光雷达的定位时刻不同步，使用点云采集左定位时刻的定位值作为此点云产生时刻的定位值，并使用两时刻的定位差作为点云配准的初值。

如图5所示，图中，上一点云采集时刻的左定位时刻为t_s，定位值为当前点云采集时刻的左定位时刻为t_e，定位值为则从上一时刻到当前时刻的定位差为此定位差即为点云配准的初值。

配准采用经典的ICP(Iterative Closest Point)算法，其核心思想是通过两点云内对应点来计算转换矩阵T_l，即：

其中，N表示对应点的个数，p_t表示目标点云，p_s表示源点云，T_l表示参考转换矩阵，T^*表示在以参考转换矩阵T_l变换源点云后求得的源点云到目标点云的转换矩阵。

令ICP算法初值T_init＝ΔT，ICP算法的主要迭代步骤包括：

1)使用上一次得到的转换矩阵T_l对源点云进行变换，如果是第一次迭代，则T_l＝T_init；

2)使用kd-tree在目标点云中找到源点云每个点的最近点；

3)通过公式(2)求解此刻的变换矩阵T^*，如果其变换幅度小于某个阈值或者迭代次数大于设定的最大迭代次数，则终止迭代，否则进行4)；

4)根据T^*更新T_l＝T^*T_l，再进行1)。

(3)局部地形地图构建

局部地形地图使用高程图来表示，高程图是一种栅格地图，栅格的索引和栅格的x、y坐标有一一对应关系，每个栅格记录着该坐标处的高度值及高度值的协方差。

机器人在每次抬脚时都会触发深度相机采集此时的地形数据，同时构建此刻局部地形地图。此时需要先将上一触发时刻的高程图转到当前时刻下，再将当前时刻的点云数据与转换后的高程图融合，构建此刻的局部地图。以图6中的两个时刻为例，如图所示，将机器人body坐标系(B₁、B₂)竖直投影到地面上，即可得到局部地形地图坐标系(M₁、M₂)；由于深度相机坐标系(C₁、C₂)与机器人body坐标系(B₁、B₂)存在一个固定的相对位姿，通过上述坐标系相对关系，即可将深度相机坐标系转换到局部地图坐标系。

在时刻1，此刻的高程图如图7(a)所示，每个栅格不同层次的灰度表示不同的高度值，先将时刻1的高程图转到时刻2，此转换矩阵为：

式中，为时刻2的机器人body坐标系转到局部地形地图坐标系的变换矩阵；为深度相机坐标系到body坐标系的转换矩阵，由于相机固连在机器人上，所以这是一个定值，可以通过机器人硬件尺寸得到；为深度相机在不同时刻的转换矩阵，可以通过上述点云配准得到；为时刻1的局部地形地图坐标系到body坐标系的转换矩阵。

对于时刻1的高程图，根据索引遍历每个栅格，栅格点的坐标为：

p_ij＝(i×d j×d h)^T (4)

式中，i、j为高程图的索引，d为高程图的分辨率，h为该栅格的高度。

栅格点的协方差为：

式中，为该栅格点在x，y方向的协方差，为该栅格点在高度方向的协方差。

根据p_ij和可将时刻1高程图内的栅格点转到时刻2：

式中，p_ij2为时刻2的栅格点坐标，为时刻2的栅格点的协方差。

根据p_ij2的坐标可以找到该栅格点在时刻2高程图中对应的栅格：

式中，x₂、y₂为p_ij2的x、y坐标，i₂、j₂为p_ij2在时刻2高程图对应栅格的索引，为取整运算。该栅格的高度值为p_ij2的z坐标，高度值的协方差为第三行第三列的值。

如图7(b)所示，高程图转换后，一部分栅格有数据，用灰色栅格表示；一部分栅格没有数据，用白色栅格表示。

将当前时刻的点云数据转到高程图的局部地形地图坐标系下：

式中，p₂为时刻2的深度相机坐标系下的点坐标；p'₂为时刻2的高程图坐标系下的点坐标；为点在深度相机坐标系下的协方差，通常由深度相机参数直接确定；为点在时刻2的高程图坐标系下的协方差。

下面需要将转换后的点加入到时刻2的高程图中，首先，通过公式(8)先找到该点在时刻2的高程图中对应的栅格，如果该栅格没有数据，那该栅格的高度为p'₂的z坐标，高度的协方差为的第三行第三列的值；如果该栅格有数据，需要将已有的数据与需要加入的数据进行融合：

式中，为栅格原有的高度值，为栅格原有高度值的协方差，p_2z为加入点的高度值(为p'₂的z坐标)，为加入点的高度值的协方差(为第三行第三列的值)，为融合后的高度值，为融合后高度值的协方差。融合后的高程图如图7(c)所示。

根据上述整个局部地形地图构造过程，便可构造出在抬脚时刻的局部地形地图。

所述实施例为本发明的优选的实施方式，但本发明并不限于上述实施方式，在不背离本发明的实质内容的情况下，本领域技术人员能够做出的任何显而易见的改进、替换或变型均属于本发明的保护范围。

Claims (10)

1.一种双足机器人导航过程中的局部地形地图构建方法，其特征在于：

深度相机不断采集地形的点云数据，由感知计算单元进行预处理后与前一帧点云进行配准，获取精准的帧间相对位姿；再根据相对位姿来更新高程图，得到局部地形地图；

配准时，使用机器人的定位信息获取两配准点云相对位姿的初值。

2.根据权利要求1所述的双足机器人导航过程中的局部地形地图构建方法，其特征在于，所述根据相对位姿来更新高程图，具体是将上一触发深度相机时刻的高程图转到当前时刻下，再将当前时刻的点云数据与转换后的高程图融合。

3.根据权利要求2所述的双足机器人导航过程中的局部地形地图构建方法，其特征在于，所述将上一触发深度相机时刻的高程图转到当前时刻下，采用如下转换矩阵：

式中，为当前时刻的机器人body坐标系转到局部地形地图坐标系的变换矩阵；为深度相机坐标系到body坐标系的转换矩阵；为深度相机在不同时刻的转换矩阵，通过点云配准得到；为上一触发深度相机时刻的局部地形地图坐标系到body坐标系的转换矩阵；其中，上一触发深度相机时刻记为时刻1，当前时刻记为时刻2。

4.根据权利要求3所述的双足机器人导航过程中的局部地形地图构建方法，其特征在于，对于高程图的转换，具体为：将时刻1高程图内的栅格点转换到时刻2，找到转换后的点在时刻2高程图中对应的栅格，把对应栅格的高度值和协方差使用转换后的值填充。

5.根据权利要求1所述的双足机器人导航过程中的局部地形地图构建方法，其特征在于，所述将时刻1高程图内的栅格点转到时刻2，先根据索引遍历时刻1的高程图的每个栅格，得到栅格点的坐标：

p_ij＝(i×d j×d h)^T

式中，i、j为高程图的索引，d为高程图的分辨率，h为栅格的高度；

栅格点的协方差为：

式中，为栅格点在x，y方向的协方差，为栅格点在高度方向的协方差；

根据p_ij和将时刻1高程图内的栅格点转换到时刻2：

式中，p_ij2为时刻2的栅格点坐标，为时刻2的栅格点的协方差。

6.根据权利要求5所述的双足机器人导航过程中的局部地形地图构建方法，其特征在于，找到转换后的点在时刻2高程图中对应的栅格，为：

式中，x₂、y₂为p_ij2的x、y坐标，i₂、j₂为p_ij2在时刻2高程图对应栅格的索引。

7.根据权利要求5所述的双足机器人导航过程中的局部地形地图构建方法，其特征在于，把对应栅格的高度值和协方差使用转换后的值填充：该栅格的高度值为p_ij2的z坐标，高度值的协方差为第三行第三列的值。

8.根据权利要求7所述的双足机器人导航过程中的局部地形地图构建方法，其特征在于，所述再将当前时刻的点云数据与转换后的高程图融合，将时刻2的点云数据转到高程图的局部地形地图坐标系下，将转换后的点加入到时刻2的高程图中。

9.根据权利要求8所述的双足机器人导航过程中的局部地形地图构建方法，其特征在于，所述将当前时刻的点云数据转到高程图的局部地形地图坐标系下，采用如下方式：

式中，p₂为时刻2的深度相机坐标系下的点坐标，p'₂为时刻2的高程图坐标系下的点坐标，为点在深度相机坐标系下的协方差，为点在时刻2的高程图坐标系下的协方差。

10.根据权利要求9所述的双足机器人导航过程中的局部地形地图构建方法，其特征在于，所述将转换后的点加入到当前时刻的高程图中，具体为：找到点在时刻2的高程图中对应的栅格，如果该栅格没有数据，那么该栅格的高度为p'₂的z坐标，高度的协方差为的第三行第三列的值；如果该栅格有数据，将时刻2的高程图中已有的数据与转换后的数据进行融合：

式中，为时刻2的高程图的栅格原有的高度值，为时刻2的高程图的栅格原有高度值的协方差；p_2z为转换后的点的高度值，即为p'₂的z坐标；为转换后的点的高度值协方差，即为第三行第三列的值，为融合后的高度值，为融合后高度值的协方差。