一种基于帧-地图匹配的高程图构建方法技术

本发明专利技术公开了一种基于帧

【技术实现步骤摘要】
一种基于帧
‑
地图匹配的高程图构建方法


[0001]本专利技术属于移动机器人地图构建领域，尤其涉及一种基于帧
‑
地图匹配的高程图构建方法。

技术介绍

[0002]地图构建是移动机器人对环境感知的一种方式，目前主流的地图表示形式包括栅格地图、点云地图、特征地图、拓扑地图等。
[0003]对于移动机器人来说，实现智能化首先需要确保机器人的自主性，而自主性的实现主要基于机器人对外部环境的感知。在未知的环境中，机器人先验知识不足，具备对环境的感知能力，是其实现环境建模、定位导航、路径规划等功能的最基本的前提。目前，SLAM(Simultaneous Localization and Mapping，同步定位与建图)技术作为移动机器人地图构建的主流方法，一定程度上赋予了机器人对于外部环境的感知能力，被广泛应用于服务机器人、巡检机器人等移动机器人上。
[0004]常规的SLAM方法构建出来的一般是稀疏点云地图，这种地图未包含完整的环境结构信息，无法反映出真实的外部环境，不能被直接用于移动机器人自主导航规划。自主导航规划的实现，一般是在稠密的栅格地图上完成的。而包含地形高程信息的2.5D栅格高程图，作为新型的栅格地图，与传统的2D平面栅格地图相比，能够为机器人的自主导航规划提供更完整的周围环境建模。同时，相比一般的三维环境建模方法，高程图本身的存储空间也较小。近年来，由于高程图的这些优点，它被广泛应用于移动机器人的全局或局部导航规划当中。
[0005]随着腿足机器人自主化程度不断提升，高程图构建的精度有了更高的要求。在实际应用中，腿足机器人的自主导航规划涉及到落足点的选择，为确保运动平稳，机器人需要避开特殊的地形区域，选择平坦的区域落足。而准确选择落足点的前提就是高精度且准确的地图。如何提高高程图构建的精度，使之准确反映真实地形，有很多需要探索的地方。目前主流的高程图构建方法受限于机器人的定位精度，无法保证局部地形的构建精度。

技术实现思路

[0006]本专利技术的目的在于针对现有技术的不足，提供一种基于帧
‑
地图匹配的高程图构建方法。
[0007]本专利技术的目的是通过以下技术方案来实现的：一种基于帧
‑
地图匹配的高程图构建方法，首先通过点云采集设备获取RGB
‑
D点云，同时从定位系统中获取当前机器人位姿；并对当前点云帧进行滤波、坐标变换的操作；再对当前点云帧和已构建的高程图进行特征提取；再是对当前点云帧进行位姿的调整；更新局部高程图，将经过位姿调整的输入点云加入局部高程图，并发布已更新的局部高程图；判断当前帧是否满足关键帧的条件，若符合关键帧的条件，就进入全局高程图的处理流程，否则回到第一步获取下一帧的RGB
‑
D点云；进行全局高程图的处理，对局部的高程图进行位姿的调整；再将局部高程图加入到全局高程
图中，最后发布全局高程图。
[0008]进一步地，所述对当前点云帧进行滤波、坐标变换的操作，具体为：
[0009](1.1)对输入点云进行直通滤波，保留点云采集设备有效量程内的点云。
[0010](1.2)对输入点云进行体素滤波降采样，使点云的分辨率与高程图栅格分辨率一致。
[0011](1.3)对滤波后的点云进行坐标变换，基于当前的定位信息，将点云采集设备坐标系下的点云变换到世界坐标系下，将其映射到地图坐标系下。
[0012]进一步地，所述对当前点云帧和已构建的高程图进行特征提取，具体为平面点提取：对点云进行法向量与曲率估计，并筛选出符合阈值要求的点，作为经过平面特征提取的平面内点。
[0013]进一步地，所述对当前点云帧进行位姿的调整，具体为：
[0014](3.1)点云配准：将经过特征提取的当前点云帧作为源点云，将经过特征提取的已构建高程图作为目标点云，使用GICP方法进行两块点云的配准，求出两者之间的位姿变换。
[0015](3.2)故障判断：求得的位姿变换矩阵需要同时满足平移距离和yaw变化角度小于一定阈值的条件，才可被判断为有效配准结果。若未被判定为有效配准结果，则回到步骤(3.1)。
[0016](3.3)点云调整：根据有效配准结果提供的位姿变换矩阵，将当前输入点云帧移动到优化后的位置。
[0017]进一步地，所述更新局部高程图，具体为：
[0018](4.1)计算高程测量值：地形的高程测量值服从高斯分布。地形的高程测量值经过调整后的原输入点云中的每一点在地图坐标系下的z坐标值都代表一个高程测量值的均值，而高程测量值的方差可由以下计算公式得出。
[0019][0020]其中，J
s
表示点云采集设备测量的雅可比矩阵；J
q
表示点云采集设备坐标系绕地图坐标系旋转的雅可比矩阵；∑
s
表示点云采集设备测量的协方差矩阵；∑
p,q
表示点云采集设备坐标系绕惯性坐标系旋转的协方差矩阵；
[0021](4.2)多高度数据处理：若出现有多个高程测量值被映射到同一个栅格中的情况，分别计算各个高程测量值与已经存储的值之间的马氏距离，选取马氏距离最大的测量值，作为当前栅格的测量值，并忽略其余的测量值。
[0022](4.3)数据融合：若当前栅格内未存储高程测量值，则直接将计算得到的高程测量值加入对应的栅格中；若当前栅格内已经存储有高程测量值，则采用卡尔曼滤波，将新计算得到的高程测量值与已储存值进行融合。
[0023]进一步地，所述进行全局高程图的处理，具体为：
[0024](5.1)关键帧判断：关键帧的选取以机器人运动的累积位移为标准，机器人运动的累积位移由下式计算得到：
[0025][0026]其中，(x
i
,y
i
)为上一时刻由定位系统输出的二维平面定位数据，(x
i+1
,y
i+1
)为当前
时刻由定位系统输出的二维平面定位数据。当机器人累计位移大于等于阈值时，将当前点云帧判定为关键帧，继续执行步骤(5.2)；若不是关键帧，则不更新全局高程图。
[0027](5.2)点云配准：将经过体素滤波降采样的局部高程图作为源点云，将用于定位的已构建全局点云图作为目标点云，使用ICP方法进行两块点云的配准，初始位姿设置为单位矩阵，求出两者之间的位姿变换。
[0028](5.3)局部高程图调整：根据有效配准结果提供的位姿变换矩阵，将局部高程图移动到优化后的位置，并复制到全局高程图中。
[0029]进一步地，机器人定位方法为使用多线激光雷达的hdl_localization。
[0030]本专利技术的有益效果是：本专利技术使用2.5D高程图作为地图的表示形式，与传统的2D平面栅格地图相比，能够为机器人的自主导航规划提供更完整的周围环境建模。同时，相比一般的三维环境建模方法，其存储空间更小，遍历速度更快；在高程图构建过程中加入了帧本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种基于帧
‑
地图匹配的高程图构建方法，其特征在于，首先通过点云采集设备获取RGB
‑
D点云，同时从定位系统中获取当前机器人位姿；并对当前点云帧进行滤波、坐标变换的操作；再对当前点云帧和已构建的高程图进行特征提取；再是对当前点云帧进行位姿的调整；更新局部高程图，将经过位姿调整的输入点云加入局部高程图，并发布已更新的局部高程图；判断当前帧是否满足关键帧的条件，若符合关键帧的条件，就进入全局高程图的处理流程，否则回到第一步获取下一帧的RGB
‑
D点云；进行全局高程图的处理，对局部的高程图进行位姿的调整；再将局部高程图加入到全局高程图中，最后发布全局高程图。2.如权利要求1所述基于帧
‑
地图匹配的高程图构建方法，其特征在于，所述对当前点云帧进行滤波、坐标变换的操作，具体为：(1.1)对输入点云进行直通滤波，保留点云采集设备有效量程内的点云。(1.2)对输入点云进行体素滤波降采样，使点云的分辨率与高程图栅格分辨率一致。(1.3)对滤波后的点云进行坐标变换，基于当前的定位信息，将点云采集设备坐标系下的点云变换到世界坐标系下，将其映射到地图坐标系下。3.如权利要求1所述基于帧
‑
地图匹配的高程图构建方法，其特征在于，所述对当前点云帧和已构建的高程图进行特征提取，具体为平面点提取：对点云进行法向量与曲率估计，并筛选出符合阈值要求的点，作为经过平面特征提取的平面内点。4.如权利要求1所述基于帧
‑
地图匹配的高程图构建方法，其特征在于，所述对当前点云帧进行位姿的调整，具体为：(3.1)点云配准：将经过特征提取的当前点云帧作为源点云，将经过特征提取的已构建高程图作为目标点云，使用GICP方法进行两块点云的配准，求出两者之间的位姿变换。(3.2)故障判断：求得的位姿变换矩阵需要同时满足平移距离和yaw变化角度小于一定阈值的条件，才可被判断为有效配准结果。若未被判定为有效配准结果，则回到步骤(3.1)。(3.3)点云调整：根据有效配准结果提供的位姿变换矩阵，将当前输入点云帧移动到优化后的位置。5.如权利要求1所述基于帧
‑
地图匹配的高程图构建方法，其特征在于，所述更新局部高程图，具体为...

【专利技术属性】
技术研发人员：朱秋国，张呈阳，吴俊，熊蓉，
申请(专利权)人：浙江大学，
类型：发明
国别省市：