一种足式机器人高程估计方法、装置及系统

1.本发明涉及激光雷达环境感知技术领域，具体而言，涉及一种足式机器人高程估计方法、装置及系统。


背景技术：

2.环境感知技术一直都是非常经典的研究热点之一，诸多学者和研究机构针对环境感知中的科学问题进行了深入研究。环境感知算法针对不同的应用场景有着不同的研究侧重点，比如测绘、ar/vr等领域，就需要将实际环境的几何、色彩等特征细节尽可能详细得展示出来，对实时性要求可以不用太高，而对于足式机器人运动规划而言，环境感知需要更侧重于实际环境中的高度信息。
3.足式机器人的运动行为需要多模块同时控制，包括激光雷达的环境感知模块、路径规划模块以及足端轨迹的步态控制模块。其中环境感知模块是连接环境与机器人的枢纽，能够在足式机器人行走过程中不断获取环境点云高度信息，为机器人的路径选择以及足端落脚需要提供有力保障。
4.为满足野外环境下足式机器人足端落点规划的需求和机器人全局路径规划及实时避障需求，需要一种能够精细化反应地面起伏高度的高程估计方法。


技术实现要素：

5.为了改善上述问题，本发明提供了一种足式机器人高程估计方法、装置及系统，通过利用激光雷达来精细化反应环境高程信息，满足足式机器人足端落点规划的需求和机器人全局路径规划及实时避障需求。
6.本发明实施例的第一方面，提供了一种足式机器人高程估计方法，所述方法包括：
7.获取各传感器采集的数据，对采集到的数据进行预处理，得到预处理后的点云数据；
8.对点云数据分割聚类，得到分类好的点云簇；
9.对点云簇进行特征提取，获取对应的平面特征和边缘特征；
10.针对不同特征的点云簇，进行特征的帧间点云匹配，计算得到相邻帧间足式机器人的相对位姿变换信息；
11.将帧间变换后的足式机器人位姿信息与数据处理后的点云融合，进行高程图的实时更新。
12.可选地，所述各传感器采集的数据包括激光雷达的点云数据和轮式里程计数据，所述获取各传感器采集的数据，对采集到的数据进行预处理，得到预处理后的点云数据的步骤，具体包括：
13.获取激光雷达采集的周围环境的点云数据；
14.获取轮式里程计采集的机器人运动过程位姿信息,进行点云去畸变处理；
15.基于足式机器人的自身高度，将采集到的高于足式机器人自身高度50％的点云数
据打上特定标签，进行滤除处理。
16.可选地，所述对点云数据分割聚类，得到分类好的点云簇的步骤，具体包括：
17.判断点云是否为地面点，将预处理后的点云数据分割为地面点云和非地面点云；
18.对分割后的非地面点云进行聚类，将代表同一环境的点云簇划分为同一类别。
19.可选地，判断点云是否为地面点的方法为：
20.计算相邻线束间的角度差，当角度差大于设定的阈值时，则将该点标记为非地面点，否则标记为地面点。
21.可选地，所述方法还包括：
22.完成所有的点云分类之后，将包含点云数量小于30个的点云簇进行标记滤除。
23.可选地，所述对点云簇进行特征提取，获取对应的平面特征和边缘特征的步骤，具体包括：
24.将雷达扫描的每一线划分为6等份扫描子区域，每份数据之间为60
°
的夹角；
25.在水平方向上将距离图像分割成几个相等的子图像，计算子图像的平滑度大小；
26.当子图像的平滑度大于设定的阈值时，标记为边缘点，当子图像的平滑度小于阈值时，标记为平面点。
27.可选地，所述针对不同特征的点云簇，进行特征的帧间点云匹配的步骤，具体包括：
28.针对6等份扫描子区域内的点云簇建立正态分布网格的概率密度函数；
29.初始化评估函数的自变量p，p为匹配希望获得的变换矩阵；
30.将同一个子区域的相邻帧点云簇按照p变换；
31.计算每个连续评估函数；
32.使用牛顿法建立对p的更新；
33.循环执行上述步骤，直到完成所有子区域的点云匹配。
34.可选地，所述将帧间变换后的足式机器人位姿信息与数据处理后的点云融合，进行高程图的实时更新的步骤，具体包括：
35.将激光雷达捕获到的环境信息映射到高程图中；
36.将足式机器人实时的位姿变换情况融合到高程图中；
37.得到具有环境高度变化的可视化栅格图。
38.本发明实施例的第二方面，提供了一种足式机器人高程估计装置，所述装置包括：
39.预处理单元，用于获取各传感器采集的数据，对采集到的数据进行预处理，得到预处理后的点云数据；
40.分割聚类单元，用于对点云数据分割聚类，得到分类好的点云簇；
41.特征提取单元，用于对点云簇进行特征提取，获取对应的平面特征和边缘特征；
42.特征匹配单元，用于针对不同特征的点云簇，进行特征的帧间点云匹配，计算得到相邻帧间足式机器人的相对位姿变换信息；
43.高程估计单元，用于将帧间变换后的足式机器人位姿信息与数据处理后的点云融合，进行高程图的实时更新。
44.本发明实施例的第三方面，提供了一种足式机器人高程估计系统，所述系统包括激光雷达数据采集模块、数据处理模块、无线数据传输模块、上位机平台，其中：
45.所述激光雷达数据采集模块，用于采集目标的原始雷达点云数据；
46.所述数据处理模块，用于执行如权利要求1-8任意一项所述的足式机器人高程估计方法；
47.所述无线数据传输模块，用于在激光雷达数据采集模块与所述上位机平台之间传输数据与指令，进行通信；
48.所述上位机平台，用于显示环境高程估计结果，并在有地面障碍物高度变化时控制外部设备实现足式机器人的步态调整。
49.综上所述，本发明提供了一种足式机器人高程估计方法、装置及系统，首先将采集到的激光雷达点云信息进行预处理，然后将点云划分为地面点和非地面点，在非地面点中提取边缘特征点和面特征点，之后通过正态分布变换匹配点云数据，通过计算得到相邻帧间足式机器人的相对位姿变换，将带有高度信息的点云数据映射到高程图中，建立可视化的栅格图。该方法能够在建立三维点云地图、获取以机器人为中心的当前环境定位信息的同时获取机器人周围环境的高度信息，并精细化的反映出局部环境中的高度变化，满足机器人足端落点规划的需求和机器人全局路径规划及实时避障需求。
附图说明
50.为了更清楚地说明本发明实施例的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，应当理解，以下附图仅示出了本发明的某些实施例，因此不应被看作是对范围的限定，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他相关的附图。
51.图1为本发明实施例的足式机器人高程估计方法的方法流程图；
52.图2为本发明实施例的进行特征的帧间点云匹配的方法流程图；
53.图3为本发明实施例的足式机器人位姿信息与数据处理后的点云融合的方法流程图；
54.图4为本发明实施例的点云数据映射到高程图中的可视化展示图。
55.图5为本发明实施例的足式机器人高程估计装置的功能模块框图。
56.图6为本发明实施例的足式机器人高程估计系统的架构示意图。
57.附图标记：
58.预处理单元110；分割聚类单元120；特征提取单元130；特征匹配单元140；高程估计单元150；激光雷达数据采集模块210、数据处理模块220、无线数据传输模块230、上位机平台240。
具体实施方式
59.环境感知技术一直都是非常经典的研究热点之一，诸多学者和研究机构针对环境感知中的科学问题进行了深入研究。环境感知算法针对不同的应用场景有着不同的研究侧重点，比如测绘、ar/vr等领域，就需要将实际环境的几何、色彩等特征细节尽可能详细得展示出来，对实时性要求可以不用太高，而对于足式机器人运动规划而言，环境感知需要更侧重于实际环境中的高度信息。
60.足式机器人的运动行为需要多模块同时控制，包括激光雷达的环境感知模块、路
径规划模块以及足端轨迹的步态控制模块。其中环境感知模块是连接环境与机器人的枢纽，能够在足式机器人行走过程中不断获取环境点云高度信息，为机器人的路径选择以及足端落脚需要提供有力保障。
61.为满足野外环境下足式机器人足端落点规划的需求和机器人全局路径规划及实时避障需求，需要一种能够精细化反应地面起伏高度的高程估计方法。
62.鉴于此，本发明设计者设计了一种足式机器人高程估计方法、装置及系统，能够在建立三维点云地图、获取以机器人为中心的当前环境定位信息的同时获取机器人周围环境的高度信息，并精细化的反映出局部环境中的高度变化，满足机器人足端落点规划的需求和机器人全局路径规划及实时避障需求。
63.为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。通常在此处附图中描述和示出的本发明实施例的组件可以以各种不同的配置来布置和设计。
64.因此，以下对在附图中提供的本发明的实施例的详细描述并非旨在限制要求保护的本发明的范围，而是仅仅表示本发明的选定实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
65.应注意到：相似的标号和字母在下面的附图中表示类似项，因此，一旦某一项在一个附图中被定义，则在随后的附图中不需要对其进行进一步定义和解释。
66.在本发明的描述中，需要说明的是，术语“顶”、“底”、“内”、“外”等指示的方位或位置关系为基于附图所示的方位或位置关系，或者是该发明产品使用时惯常摆放的方位或位置关系，仅是为了便于描述本发明和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本发明的限制。此外，术语“第一”、“第二”等仅用于区分描述，而不能理解为指示或暗示相对重要性。
67.在本发明的描述中，还需要说明的是，除非另有明确的规定和限定，术语“设置”、“安装”、“相连”、“连接”应做广义理解，例如，可以是固定连接，也可以是可拆卸连接，或一体地连接；可以是机械连接，也可以是电连接；可以是直接相连，也可以通过中间媒介间接相连，可以是两个元件内部的连通。对于本领域的普通技术人员而言，可以具体情况理解上述术语在本发明中的具体含义。
68.需要说明的是，在不冲突的情况下，本发明中的实施例及实施例中的特征可以相互组合。
69.请参阅图1，为本发明一实施例提供的足式机器人高程估计方法，该方法包括：
70.步骤s101,获取各传感器采集的数据，对采集到的数据进行预处理，得到预处理后的点云数据。
71.其中，传感器数据包括激光雷达点云数据、gps定位数据以及imu传感器数据。通过预处理后，将点云数据中的畸变和不参与计算的部分进行滤除。
72.作为本发明实施例的优选实施方式，所述各传感器采集的数据包括激光雷达的点云数据和imu传感器获得的轮式里程计数据。步骤s101具体包括：
73.获取激光雷达采集的周围环境的点云数据；获取轮式里程计采集的机器人运动过
程位姿信息,进行点云去畸变处理；基于足式机器人的自身高度，将采集到的高于足式机器人自身高度50％的点云数据打上特定标签，进行滤除处理。
74.其中，点云去畸变处理的具体方式：通过加入轮式里程计(wheel odometry)进行辅助，用中央处理器(central processing unit，cpu)读取激光雷达数据，同时单片机上传里程计积分数据，两者时间同步，利用旋转矩阵将该时刻的雷达数据转换到里程计坐标系下，通过此方式可以计算出该时间每个点在全局坐标系下的准确位置，通过变换可以尽量减小激光雷达运动畸变造成的点云数据失真，里程计插值补偿公式可表示为：
[0075][0076]
其中，为旋转角度，为激光雷达扫描的某个点在里程计坐标系下的坐标，为激光雷达自身在里程计坐标系下的坐标，为上述点与激光雷达间的距离。
[0077]
将高于足式机器人50％的点云数据标记为1，用于判断机器人的避障需求，将不高于足式机器人30％的点云数据标记为2，用于判断机器人的足端落脚需求。
[0078]
通过上述的预处理过程处理后，得到的点云数据进入下一个处理步骤。
[0079]
作为本发明实施例的优选实施方式，为使雷达能够达到最佳的测量效果，将激光雷达安装在六足机器人上，该激光雷达选用velodyne-16三维激光雷达，其测量的范围可达100米，精度为
±
3cm，扫描范围为垂直方向30
°
(
±
15
°
)和水平方向360
°
，其频率为5hz～20hz。
[0080]
步骤s102，对点云数据分割聚类，得到分类好的点云簇。将预处理后的点云数据做分割聚类，首先将点云划分为地面点和非地面点，再通过计算相邻点云间的空间相对夹角，将非地面点聚类成特征的点云簇。
[0081]
作为本发明实施例的优选方式，步骤s102，具体包括：
[0082]
判断点云是否为地面点，将预处理后的点云数据分割为地面点云和非地面点云；对分割后的非地面点云进行聚类，将代表同一环境的点云簇划分为同一类别。
[0083]
判断点云是否为地面点的具体方式为：
[0084]
通过计算不同线束相邻两点的俯仰角大小，判断该点是否属于地面点，其计算公式为：
[0085][0086]
其中angle表示两点之间的俯仰角，diffy代表不同线束间相邻两点在y轴上的距离差，diffz代表不同线束间相邻两点在z轴上的距离差，通过该式可以计算出相邻线束间的角度差，当角度差angle大于设定的阈值时，则该点标记为非地面点，否则标记为地面点。
[0087]
对分割后的非地面点云进行聚类的具体方式为：
[0088]
将标记好的非地面点进行分类：通过计算雷达扫描的连续两个点云，并计算两者之间的相对夹角，若两个点之间的夹角大于设定的阈值时，则标记该两点不在同一点云簇，若两个点之间的夹角小于设定的阈值时，则标记该两点属于同一点云簇。
[0089]
作为本发明实施例的优选方式，为实现快速、可靠的特征提取，完成所有的点云分类之后，将包含点云数量小于30个的点云簇进行标记滤除。
[0090]
步骤s103，对点云簇进行特征提取，获取对应的平面特征和边缘特征。本步骤的主要穆伟是雷达点云特征提取,特征提取用以获得独特的平面特征和边缘特征。
[0091]
具体的操作方式为：
[0092]
对分类好的点云簇进行特征提取：在水平方向上将距离图像分割成几个相等的子图像，通过公式计算子图像的平滑度大小：
[0093][0094]
其中s代表参与计算点数，r
jri
分别表示集合s中的点到激光雷达的欧式距离。
[0095]
提取时将雷达扫描的每一线划分为6等份，每份数据之间为60
°
的夹角，当子图像的平滑度大于设定的阈值时，标记为边缘点，当子图像的平滑度小于阈值时，标记为平面点。针对边缘点和平面点，分别提取平面特征和边缘特征。
[0096]
步骤s104，针对不同特征的点云簇，进行特征的帧间点云匹配，计算得到相邻帧间足式机器人的相对位姿变换信息。针对不同平面特征和边缘特征的点云簇，通过计算协方差矩阵的特征值和特征向量，完成特征的完成特征的线-线、面-面的帧间点云匹配。
[0097]
作为本发明实施例的优选实施方式，如图2所示，步骤s104具体包括：
[0098]
步骤s201，针对6等份扫描子区域内的点云簇建立正态分布网格的概率密度函数；
[0099]
具体的执行方式为：针对6等份子区域，将每份子区域在三维空间上划分为网格，并针对点云数量大于5的网格，利用点云的空间坐标计算均值向量m和协方差矩阵∑，其计算公式为：
[0100][0101][0102]
其中，xi表示网格中的各点云空间坐标，t表示数据维数。
[0103]
则每一个网格中的概率密度函数为：
[0104][0105]
步骤s202，初始化评估函数的自变量p，p为匹配希望获得的变换矩阵；
[0106]
具体地，基于每个网格中的概率密度函数，建立评估函数score(p)，其计算公式为：
[0107][0108]
其中，表示网格i中的点云向量，表示网格i中的点云均值向量，p是自变量，即匹配
需要的得到的变换矩阵，由旋转矩阵和平移向量组成：
[0109][0110][0111]
其中，表示旋转角度，表示网格点到激光雷达的距离，表示变换后网格点在世界坐标系下的坐标，表示网格点在激光雷达坐标系下的坐标。
[0112]
步骤s203，将同一个子区域的相邻帧点云簇按照p变换。
[0113]
步骤s204，计算每个连续评估函数；
[0114]
步骤s205，使用牛顿法建立对p的更新；
[0115]
具体地，用牛顿法对参数进行优化，实现对自变量p的更新：
[0116]
p
←
p+δp
[0117]
hδp＝-g
[0118]
其中h为hessian矩阵，g为梯度向量，每次迭代求解出的增量δp加到当前的转换向量p中；其中g中元素为：
[0119][0120]
hessian矩阵h中元素为：
[0121][0122]
步骤s206，循环执行上述步骤步骤s203，直到完成所有子区域的点云匹配。
[0123]
步骤s105，将帧间变换后的足式机器人位姿信息与数据处理后的点云融合，进行高程图的实时更新。更新的方式为，通过将激光雷达实时捕获到的环境信息映射到高程地图中，得到真实环境地形的高度起伏状态，并加入足式机器人实时的位姿变换，最终得到具有环境高度变化的可视化栅格图。
[0124]
作为本发明实施例的优选实施方式，如图3所示，步骤s105具体包括。
[0125]
步骤s301，将激光雷达捕获到的观测信息映射到高程图中；
[0126]
具体地，高程图中的网格单元i的三维位置由pi＝(xi，yi，hi)给出，其中xi和yi由网格单元i的位置定义，而hi是该单元处的地形估计高度。
[0127]
当来自激光雷达捕获到的新数据被处理为空间中的点，并被映射到高程图时，这导致在高程图的某个单元网格上有一个新的高度测量值hi(假设每个距离测量点更新一次单元高度)；
[0128]
那么在第m帧时，高程图的高度测量值由高斯概率分布近似为其中是映射到高程图坐标系后的高；p为测量均值；为测量方差，由方差传播计算得到：
[0129]
[0130]
其中表示激光雷达模型的协方差矩阵，该值从激光雷达的噪声模型中获得，表示激光雷达旋转的协方差矩阵(的子矩阵)。
[0131]
式中：
[0132][0133][0134]
此时，在基于激光雷达坐标系下，观测到点p的环境高度可表示为：
[0135][0136]
其中为激光雷达s到p点的距离sp在s坐标系下的表示；为激光雷达s到高程图坐标系原点m的距离在m坐标系下的表示；p是映射阵：p＝[0,0,1]；
[0137]
最后通过一维卡尔曼滤波器将高度测量值与现有的高程估计值融合，完成对激光雷达观测信息在高程图上的映射。
[0138]
步骤s302，将足式机器人实时的位姿变换情况融合到高程图中。通过将机器人移动融合到栅格地图中，实现了将位姿估计不确定性体现到栅格图中。
[0139]
具体地，由于高程图坐标系是根据激光雷达坐标系以及机器人的运动位姿定义的，高程图的数据应该随着足式机器人的运动而实时更新。
[0140]
当网格单元i接收到测量更新时，则反映真实地形的空间协方差矩阵可表示为：
[0141][0142]
其中和是来自网格离散化的水平不确定性的近似值，对于边长为d的正方形单元网格时，
[0143]
当网格单元i没有接收到激光雷达新的测量更新时，协方差矩阵则由足式机器人从先前姿势到当前姿势的相对运动来连续更新。
[0144]
步骤s303，得到具有环境高度变化的可视化栅格图。通过上述步骤s301和s302中的自定义概率栅格地图，确定每个点各自的期望、方差以及置信上下界，便能导出常见的二维栅格图。
[0145]
具体地，当作地图融合变换时，将高程图数据结构转换为每个单元i的表示其中平均高度为单元i的xy平面中2σ置信区间内所有单元的加权平均值，表示高度估计值的置信下限和置信上限，使得真实的高度在95％的置信区间中。
[0146]
如图4所示，图4为六足机器人接收到包含高度信息的激光雷达观测点云数据映射到高程图中的可视化展示，由高程图可以看出装载激光雷达的六足机器人可以精细化反映
出局部环境的高程变化，并随着六足机器人的相对位姿变化，实时更新高程地图。
[0147]
综上，本实施例提供的足式机器人高程估计方法，首先将采集到的激光雷达点云信息中高于机器人自身50％的点云滤除；将聚类后点云数量小于30的点云簇标记为噪声滤除。在对点云预处理后，将点云划分为地面点和非地面点，在非地面点中提取边缘特征点和面特征点，之后通过正态分布变换匹配点云数据，通过计算得到相邻帧间足式机器人的相对位姿变换，将带有高度信息的点云数据映射到高程图中，建立建立可视化的2.5d栅格图。该方法能够在建立三维点云地图、获取以机器人为中心的当前环境定位信息的同时获取机器人周围环境的高度信息，并精细化的反映出局部环境中的高度变化，满足机器人足端落点规划的需求和机器人全局路径规划及实时避障需求。
[0148]
如图5所示，本发明实施提供的足式机器人高程估计装置，所述装置包括：
[0149]
预处理单元110，用于获取各传感器采集的数据，对采集到的数据进行预处理，得到预处理后的点云数据；
[0150]
分割聚类单元120，用于对点云数据分割聚类，得到分类好的点云簇；
[0151]
特征提取单元130，用于对点云簇进行特征提取，获取对应的平面特征和边缘特征；
[0152]
特征匹配单元140，用于针对不同特征的点云簇，进行特征的帧间点云匹配，计算得到相邻帧间足式机器人的相对位姿变换信息；
[0153]
高程估计单元150，用于将帧间变换后的足式机器人位姿信息与数据处理后的点云融合，进行高程图的实时更新。
[0154]
本发明实施例提供的足式机器人高程估计装置，用于实现上述足式机器人高程估计方法，因此具体实施方式与上述方法相同，在此不再赘述。
[0155]
如图6所示，本发明实施提供的足式机器人高程估计系统，所述系统包括激光雷达数据采集模块210、数据处理模块220、无线数据传输模块230、上位机平台240，其中：
[0156]
所述激光雷达数据采集模块210，用于采集目标的原始雷达点云数据；
[0157]
所述数据处理模块220，用于执行上述的足式机器人高程估计方法；
[0158]
所述无线数据传输模块230，用于在激光雷达数据采集模块210与所述上位机平台240之间传输数据与指令，进行通信；
[0159]
所述上位机平台240，用于显示环境高程估计结果，并在有地面障碍物高度变化时控制外部设备实现足式机器人的步态调整。
[0160]
综上所述，本发明提供了一种足式机器人高程估计方法、装置及系统，首先将采集到的激光雷达点云信息进行预处理，然后将点云划分为地面点和非地面点，在非地面点中提取边缘特征点和面特征点，之后通过正态分布变换匹配点云数据，通过计算得到相邻帧间足式机器人的相对位姿变换，将带有高度信息的点云数据映射到高程图中，建立可视化的栅格图。该方法能够在建立三维点云地图、获取以机器人为中心的当前环境定位信息的同时获取机器人周围环境的高度信息，并精细化的反映出局部环境中的高度变化，满足机器人足端落点规划的需求和机器人全局路径规划及实时避障需求。
[0161]
在本技术所公开的几个实施例中，应该理解到，所揭露的装置和方法，也可以通过其它的方式实现。以上所描述的装置实施例仅仅是示意性的，例如，附图中的流程图和框图显示了根据本技术的多个实施例的装置、方法和计算机程序产品的可能实现的体系架构、
功能和操作。在这点上，流程图或框图中的每个方框可以代表一个模块、程序段或代码的一部分，模块、程序段或代码的一部分包含一个或多个用于实现规定的逻辑功能的可执行指令。也应当注意，在有些作为替换的实现方式中，方框中所标注的功能也可以以不同于附图中所标注的顺序发生。例如，两个连续的方框实际上可以基本并行地执行，它们有时也可以按相反的顺序执行，这依所涉及的功能而定。也要注意的是，框图和/或流程图中的每个方框、以及框图和/或流程图中的方框的组合，可以用执行规定的功能或动作的专用的基于硬件的系统来实现，或者可以用专用硬件与计算机指令的组合来实现。
[0162]
另外，在本技术各个实施例中的各功能模块可以集成在一起形成一个独立的部分，也可以是各个模块单独存在，也可以两个或两个以上模块集成形成一个独立的部分。
[0163]
所述功能如果以软件功能模块的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读存储介质中。基于这样的理解，本技术的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)执行本技术各个实施例所述方法的全部或部分步骤。而前述的存储介质包括：u盘、移动硬盘、只读存储器(rom，read-only memory)、随机存取存储器(ram，random access memory)、磁碟或者光盘等各种可以存储程序代码的介质。

技术特征：
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任意一项所述的足式机器人高程估计方法；所述无线数据传输模块，用于在激光雷达数据采集模块与所述上位机平台之间传输数据与指令，进行通信；所述上位机平台，用于显示环境高程估计结果，并在有地面障碍物高度变化时控制外部设备实现足式机器人的步态调整。

技术总结
本发明提供了一种足式机器人高程估计方法、装置及系统，首先将采集到的激光雷达点云信息进行预处理，然后将点云划分为地面点和非地面点，在非地面点中提取边缘特征点和面特征点，之后通过正态分布变换匹配点云数据，通过计算得到相邻帧间足式机器人的相对位姿变换，将带有高度信息的点云数据映射到高程图中，建立可视化的栅格图。该方法能够在建立三维点云地图、获取以机器人为中心的当前环境定位信息的同时获取机器人周围环境的高度信息，并精细化的反映出局部环境中的高度变化，满足机器人足端落点规划的需求和机器人全局路径规划及实时避障需求。实时避障需求。实时避障需求。


技术研发人员：胡川妹 李治宏 蒋刚 邹琴 范孝冬 蔡良轩 杜冬 李超
受保护的技术使用者：成都理工大学
技术研发日：2023.05.17
技术公布日：2023/8/4