[发明专利]一种足式机器人高程估计方法、装置及系统

权利要求书

1.一种足式机器人高程估计方法，其特征在于，所述方法包括：

获取各传感器采集的数据，对采集到的数据进行预处理，得到预处理后的点云数据；

对点云数据分割聚类，得到分类好的点云簇；

对点云簇进行特征提取，获取对应的平面特征和边缘特征；

针对不同特征的点云簇，进行特征的帧间点云匹配，计算得到相邻帧间足式机器人的相对位姿变换信息；

将帧间变换后的足式机器人位姿信息与数据处理后的点云融合，进行高程图的实时更新。

2.根据权利要求1所述的足式机器人高程估计方法，其特征在于，所述各传感器采集的数据包括激光雷达的点云数据和轮式里程计数据，所述获取各传感器采集的数据，对采集到的数据进行预处理，得到预处理后的点云数据的步骤，具体包括：

获取激光雷达采集的周围环境的点云数据；

获取轮式里程计采集的机器人运动过程位姿信息,进行点云去畸变处理；

基于足式机器人的自身高度，将采集到的高于足式机器人自身高度50％的点云数据打上特定标签，进行滤除处理。

3.根据权利要求2所述的足式机器人高程估计方法，其特征在于，所述对点云数据分割聚类，得到分类好的点云簇的步骤，具体包括：

判断点云是否为地面点，将预处理后的点云数据分割为地面点云和非地面点云；

对分割后的非地面点云进行聚类，将代表同一环境的点云簇划分为同一类别。

4.根据权利要求3所述的足式机器人高程估计方法，其特征在于，判断点云是否为地面点的方法为：

计算相邻线束间的角度差，当角度差大于设定的阈值时，则将该点标记为非地面点，否则标记为地面点。

5.根据权利要求4所述的足式机器人高程估计方法，其特征在于，所述方法还包括：

完成所有的点云分类之后，将包含点云数量小于30个的点云簇进行标记滤除。

6.根据权利要求5所述的足式机器人高程估计方法，其特征在于，所述对点云簇进行特征提取，获取对应的平面特征和边缘特征的步骤，具体包括：

将雷达扫描的每一线划分为6等份扫描子区域，每份数据之间为60°的夹角；

在水平方向上将距离图像分割成几个相等的子图像，计算子图像的平滑度大小；

当子图像的平滑度大于设定的阈值时，标记为边缘点，当子图像的平滑度小于阈值时，标记为平面点。

7.根据权利要求6所述的足式机器人高程估计方法，其特征在于，所述针对不同特征的点云簇，进行特征的帧间点云匹配的步骤，具体包括：

针对6等份扫描子区域内的点云簇建立正态分布网格的概率密度函数；

初始化评估函数的自变量p，p为匹配希望获得的变换矩阵；

将同一个子区域的相邻帧点云簇按照p变换；

计算每个连续评估函数；

使用牛顿法建立对p的更新；

循环执行上述步骤，直到完成所有子区域的点云匹配。

8.根据权利要求7所述的足式机器人高程估计方法，其特征在于，所述将帧间变换后的足式机器人位姿信息与数据处理后的点云融合，进行高程图的实时更新的步骤，具体包括：

将激光雷达捕获到的环境信息映射到高程图中；

将足式机器人实时的位姿变换情况融合到高程图中；

得到具有环境高度变化的可视化栅格图。

9.一种足式机器人高程估计装置，其特征在于，所述装置包括：

预处理单元，用于获取各传感器采集的数据，对采集到的数据进行预处理，得到预处理后的点云数据；

分割聚类单元，用于对点云数据分割聚类，得到分类好的点云簇；

特征提取单元，用于对点云簇进行特征提取，获取对应的平面特征和边缘特征；

特征匹配单元，用于针对不同特征的点云簇，进行特征的帧间点云匹配，计算得到相邻帧间足式机器人的相对位姿变换信息；

高程估计单元，用于将帧间变换后的足式机器人位姿信息与数据处理后的点云融合，进行高程图的实时更新。

10.一种足式机器人高程估计系统，其特征在于，所述系统包括激光雷达数据采集模块、数据处理模块、无线数据传输模块、上位机平台，其中：

所述激光雷达数据采集模块，用于采集目标的原始雷达点云数据；

所述数据处理模块，用于执行如权利要求1-8任意一项所述的足式机器人高程估计方法；

所述无线数据传输模块，用于在激光雷达数据采集模块与所述上位机平台之间传输数据与指令，进行通信；

所述上位机平台，用于显示环境高程估计结果，并在有地面障碍物高度变化时控制外部设备实现足式机器人的步态调整。