CN113960614A - 一种基于帧-地图匹配的高程图构建方法 - Google Patents

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

Info

Publication number
CN113960614A
CN113960614A CN202111107037.0A CN202111107037A CN113960614A CN 113960614 A CN113960614 A CN 113960614A CN 202111107037 A CN202111107037 A CN 202111107037A CN 113960614 A CN113960614 A CN 113960614A
Authority
CN
China
Prior art keywords
point cloud
map
elevation
elevation map
frame
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Pending
Application number
CN202111107037.0A
Other languages
English (en)
Inventor
朱秋国
张呈阳
吴俊�
熊蓉
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Zhejiang University ZJU
Original Assignee
Zhejiang University ZJU
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Zhejiang University ZJU filed Critical Zhejiang University ZJU
Priority to CN202111107037.0A priority Critical patent/CN113960614A/zh
Publication of CN113960614A publication Critical patent/CN113960614A/zh
Pending legal-status Critical Current

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/02—Systems using the reflection of electromagnetic waves other than radio waves
    • G01S17/06—Systems determining position data of a target
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/88—Lidar systems specially adapted for specific applications
    • G01S17/89—Lidar systems specially adapted for specific applications for mapping or imaging
    • G01S17/894—3D imaging with simultaneous measurement of time-of-flight at a 2D array of receiver pixels, e.g. time-of-flight cameras or flash lidar
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
    • G05D1/08—Control of attitude, i.e. control of roll, pitch, or yaw
    • G05D1/0891—Control of attitude, i.e. control of roll, pitch, or yaw specially adapted for land vehicles
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/30—Determination of transform parameters for the alignment of images, i.e. image registration
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/10—Image acquisition modality
    • G06T2207/10028—Range image; Depth image; 3D point clouds

Abstract

本发明公开了一种基于帧‑地图匹配的高程图构建方法，使用2.5D高程图作为地图的表示形式，能够为机器人的自主导航规划提供更完整的周围环境建模。同时，存储空间更小，遍历速度更快；在高程图构建过程中加入了帧与地图的配准，减小了由于定位误差造成的地图构建偏差；在配准之前对待配准点云进行了平面特征提取，使用基于平面特征的配准，与直接法配准相比，提高了配准结果的准确性与鲁棒性；另外，将高程图的构建框架改为局部高程图和全局高程图相结合，加入了全局高程图的构建步骤，有效消除了局部高程图构建过程带来的累计误差，提高了全局建图的准确性。本发明为腿足机器人在复杂场景下的自主移动提供了足够的感知基础。

Description

一种基于帧-地图匹配的高程图构建方法
技术领域
本发明属于移动机器人地图构建领域，尤其涉及一种基于帧-地图匹配的高程图构建方法。
背景技术
地图构建是移动机器人对环境感知的一种方式，目前主流的地图表示形式包括栅格地图、点云地图、特征地图、拓扑地图等。
对于移动机器人来说，实现智能化首先需要确保机器人的自主性，而自主性的实现主要基于机器人对外部环境的感知。在未知的环境中，机器人先验知识不足，具备对环境的感知能力，是其实现环境建模、定位导航、路径规划等功能的最基本的前提。目前，SLAM(Simultaneous Localization and Mapping，同步定位与建图)技术作为移动机器人地图构建的主流方法，一定程度上赋予了机器人对于外部环境的感知能力，被广泛应用于服务机器人、巡检机器人等移动机器人上。
常规的SLAM方法构建出来的一般是稀疏点云地图，这种地图未包含完整的环境结构信息，无法反映出真实的外部环境，不能被直接用于移动机器人自主导航规划。自主导航规划的实现，一般是在稠密的栅格地图上完成的。而包含地形高程信息的2.5D栅格高程图，作为新型的栅格地图，与传统的2D平面栅格地图相比，能够为机器人的自主导航规划提供更完整的周围环境建模。同时，相比一般的三维环境建模方法，高程图本身的存储空间也较小。近年来，由于高程图的这些优点，它被广泛应用于移动机器人的全局或局部导航规划当中。
随着腿足机器人自主化程度不断提升，高程图构建的精度有了更高的要求。在实际应用中，腿足机器人的自主导航规划涉及到落足点的选择，为确保运动平稳，机器人需要避开特殊的地形区域，选择平坦的区域落足。而准确选择落足点的前提就是高精度且准确的地图。如何提高高程图构建的精度，使之准确反映真实地形，有很多需要探索的地方。目前主流的高程图构建方法受限于机器人的定位精度，无法保证局部地形的构建精度。
发明内容
本发明的目的在于针对现有技术的不足，提供一种基于帧-地图匹配的高程图构建方法。
本发明的目的是通过以下技术方案来实现的：一种基于帧-地图匹配的高程图构建方法，首先通过点云采集设备获取RGB-D点云，同时从定位系统中获取当前机器人位姿；并对当前点云帧进行滤波、坐标变换的操作；再对当前点云帧和已构建的高程图进行特征提取；再是对当前点云帧进行位姿的调整；更新局部高程图，将经过位姿调整的输入点云加入局部高程图，并发布已更新的局部高程图；判断当前帧是否满足关键帧的条件，若符合关键帧的条件，就进入全局高程图的处理流程，否则回到第一步获取下一帧的RGB-D点云；进行全局高程图的处理，对局部的高程图进行位姿的调整；再将局部高程图加入到全局高程图中，最后发布全局高程图。
进一步地，所述对当前点云帧进行滤波、坐标变换的操作，具体为：
(1.1)对输入点云进行直通滤波，保留点云采集设备有效量程内的点云。
(1.2)对输入点云进行体素滤波降采样，使点云的分辨率与高程图栅格分辨率一致。
(1.3)对滤波后的点云进行坐标变换，基于当前的定位信息，将点云采集设备坐标系下的点云变换到世界坐标系下，将其映射到地图坐标系下。
进一步地，所述对当前点云帧和已构建的高程图进行特征提取，具体为平面点提取：对点云进行法向量与曲率估计，并筛选出符合阈值要求的点，作为经过平面特征提取的平面内点。
进一步地，所述对当前点云帧进行位姿的调整，具体为：
(3.1)点云配准：将经过特征提取的当前点云帧作为源点云，将经过特征提取的已构建高程图作为目标点云，使用GICP方法进行两块点云的配准，求出两者之间的位姿变换。
(3.2)故障判断：求得的位姿变换矩阵需要同时满足平移距离和yaw变化角度小于一定阈值的条件，才可被判断为有效配准结果。若未被判定为有效配准结果，则回到步骤(3.1)。
(3.3)点云调整：根据有效配准结果提供的位姿变换矩阵，将当前输入点云帧移动到优化后的位置。
进一步地，所述更新局部高程图，具体为：
(4.1)计算高程测量值：地形的高程测量值服从高斯分布。地形的高程测量值经过调整后的原输入点云中的每一点在地图坐标系下的z坐标值都代表一个高程测量值的均值，而高程测量值的方差可由以下计算公式得出。
Figure BDA0003272834950000021
其中，Js表示点云采集设备测量的雅可比矩阵；Jq表示点云采集设备坐标系绕地图坐标系旋转的雅可比矩阵；∑s表示点云采集设备测量的协方差矩阵；∑p,q表示点云采集设备坐标系绕惯性坐标系旋转的协方差矩阵；
(4.2)多高度数据处理：若出现有多个高程测量值被映射到同一个栅格中的情况，分别计算各个高程测量值与已经存储的值之间的马氏距离，选取马氏距离最大的测量值，作为当前栅格的测量值，并忽略其余的测量值。
(4.3)数据融合：若当前栅格内未存储高程测量值，则直接将计算得到的高程测量值加入对应的栅格中；若当前栅格内已经存储有高程测量值，则采用卡尔曼滤波，将新计算得到的高程测量值与已储存值进行融合。
进一步地，所述进行全局高程图的处理，具体为：
(5.1)关键帧判断：关键帧的选取以机器人运动的累积位移为标准，机器人运动的累积位移由下式计算得到：
Figure BDA0003272834950000031
其中，(xi,yi)为上一时刻由定位系统输出的二维平面定位数据，(xi+1,yi+1)为当前时刻由定位系统输出的二维平面定位数据。当机器人累计位移大于等于阈值时，将当前点云帧判定为关键帧，继续执行步骤(5.2)；若不是关键帧，则不更新全局高程图。
(5.2)点云配准：将经过体素滤波降采样的局部高程图作为源点云，将用于定位的已构建全局点云图作为目标点云，使用ICP方法进行两块点云的配准，初始位姿设置为单位矩阵，求出两者之间的位姿变换。
(5.3)局部高程图调整：根据有效配准结果提供的位姿变换矩阵，将局部高程图移动到优化后的位置，并复制到全局高程图中。
进一步地，机器人定位方法为使用多线激光雷达的hdl_localization。
本发明的有益效果是：本发明使用2.5D高程图作为地图的表示形式，与传统的2D平面栅格地图相比，能够为机器人的自主导航规划提供更完整的周围环境建模。同时，相比一般的三维环境建模方法，其存储空间更小，遍历速度更快；在高程图构建过程中加入了帧与地图的配准，减小了由于定位误差造成的地图构建偏差；在配准之前对待配准点云进行了平面特征提取，使用基于平面特征的配准，与直接法配准相比，提高了配准结果的准确性与鲁棒性；另外，将高程图的构建框架改为局部高程图和全局高程图相结合，加入了全局高程图的构建步骤，有效消除了局部高程图构建过程带来的累计误差，提高了全局建图的准确性。另外，使用本发明构建的高程图可被直接用于腿足机器人的落足点选择与规划中，为腿足机器人在复杂场景下的自主移动提供了足够的感知基础。
附图说明
图1为本发明高程图构建方法的流程图；
图2为本发明一种实施例的系统结构示意图。
具体实施方式
本发明一种基于帧-地图匹配的高程图构建方法，实现了机器人周围环境的实时准确构建，并在腿足机器人上进行了实验测试。
图2示例性地展示了适用于本发明方法的一个实例系统结构，该结构可以分为待构建环境205以及腿足机器人204，其中腿足机器人中与本发明方法相关的主要构件为点云采集设备201、定位传感器202、主机203。其在本发明的使用示例中分别为，点云采集设备：Intel RealSense D435i，定位传感器：Velodyne VLP-16，主机：NUCi7BEH。
其中，主机实时获取点云采集设备中采集到的实时点云帧，同时，实时从定位传感器获取的数据中得到机器人的当前位姿，并在主机中构建高程图，最后发布出来。本发明的点云处理和建图过程都是在主机上完成的。
需要说明的是，上述图2的系统结构仅仅作为本发明方法使用的示例，本发明对此不做限定。
如图1所示，本发明具体包括如下步骤：
步骤S101，通过点云采集设备获取RGB-D点云，Intel RealSense D435i深度相机通过USB3.0接口连接于NUC主机上，其获取实时的分辨率为640×480的RGB图像数据，与分辨率为640×480的深度图像数据，并将两者整合为RGB-D点云数据，采集帧率为每秒30帧。
步骤S102，从定位系统中获取当前机器人在地图坐标系下的位姿。获取机器人位姿的方法为hdl_localization，基于Velodyne VLP-16激光雷达，在已构建的点云图中定位得到。
步骤S103，对当前点云帧进行滤波，并根据当前位姿对点云进行坐标变换。此步骤中对当前点云帧的处理步骤包含以下操作：
(3.1)对输入点云进行直通滤波，保留点云采集设备有效量程内的点云。IntelRealSense D435i在有效量程以外的点云采集精度较差，因此使用直通滤波，保留距离0.2m-2m范围内的点云，舍去阈值范围以外的点云。
(3.2)对输入点云进行体素滤波降采样，使点云的分辨率与高程图栅格分辨率一致。本实例中，将体素滤波的分辨率设置为2cm，与高程图的栅格分辨率一致。
(3.3)对滤波后的点云进行坐标变换，基于当前的定位信息，将点云采集设备坐标系下的点云变换到世界坐标系下，将其映射到地图坐标系下，经过映射后的点在地图坐标系下的高程测量值p可由下式计算得到：
Figure BDA0003272834950000041
P＝[0 0 1]
其中，CSM指地图坐标系与传感器坐标系之间的旋转变换阵，q为单位四元数，用于参数化该变换阵；SrSP指测量点在传感器坐标系下的位置，MrSM指传感器坐标系原点在地图坐标系下的位置。
步骤S104，对当前点云帧与已构建高程图进行特征提取，提取当前点云帧中的平面点。依据步骤S103中经过处理的当前点云帧，再对其进行特征提取。具体操作为：对点云进行法向量与曲率估计，并筛选出z轴法向量大于一定阈值的点，作为经过平面特征提取的平面内点。保留平面内点，舍弃平面外点，以待以下步骤使用。
利用本实例的特征提取方法，在配准之前对待配准点云进行平面特征提取，从而使以下步骤能够使用基于平面特征的配准，提高了配准结果的准确性与鲁棒性；
步骤S105，对当前点云帧进行位姿调整。依据步骤S104中经过特征提取的当前点云帧与已构建高程图，使用点云配准方法寻找两片点云的重叠部分，求出能使两片点云重合程度最大化的位姿变换。具体方法为：将经过特征提取的当前点云帧作为源点云，将经过特征提取的已构建高程图作为目标点云，使用GICP方法进行两片点云的配准，求解使两者之间重合程度最大化的的位姿变换，其中，求解的初始位姿设置为单位矩阵。
进一步地，由于用GICP方法求解位姿变换存在求解失败的可能性，在步骤S105中加入了故障判断的的操作，求得的位姿变换矩阵需要同时满足以下三个条件，才可被判断为有效配准结果。若未被判定为有效配准结果，则重复上述点云配准步骤。
1、水平方向变换位移<0.1m；
2、垂直方向变换位移<0.2m；
3、Yaw角变换角度<10°。
进一步地，根据有效配准结果提供的位姿变换矩阵，将原输入点云帧移动到优化后的位置。此时原输入点云帧和已构建高程图之间的重合程度最大。
利用步骤S105中的帧与地图配准方法，能够减小由于定位误差造成的地图构建偏差，使构建的高程图能够更加符合真实环境，确保了地图构建的准确性。
步骤S106，更新并发布局部高程图。局部高程图是能够存储高程信息的2.5D栅格地图。使用2.5D高程图作为地图的表示形式，与传统的2D平面栅格地图相比，能够为机器人的自主导航规划提供更完整的周围环境建模。同时，相比一般的三维环境建模方法，其存储空间更小，遍历速度更快。该地图以机器人坐标系为中心，实时更新并能够反映机器人周围的三维环境。为了确保局部高程图构建的实时性和精确性，本实例将高程图的分辨率设置为2cm，即每一个栅格单元的尺寸为2cm×2cm；将局部高程图的尺寸大小设置为5m×5m。
基于当前输入的点云帧来更新高程图，需要首先迭代地计算出点云中的每一个点在地图坐标系中所表示的高程测量值，找到每一个高程测量值所映射的栅格单元，然后根据计算得到的高程测量值与已存储在对应栅格内的历史测量值，进行数据融合，得到更新值。
进一步地，更新高程图的具体步骤为：
(6.1)计算高程测量值：本发明假定地形的高程测量值服从高斯分布
Figure BDA0003272834950000061
经过调整后的原输入点云中的每一点在地图坐标系下的z坐标值都代表一个高程测量值的均值，而高程测量值的方差
Figure BDA0003272834950000062
可由以下计算公式得出：
Figure BDA0003272834950000063
其中，Js表示点云采集设备测量的雅可比矩阵，Jq表示点云采集设备坐标系绕地图坐标系旋转的雅可比矩阵；∑s表示点云采集设备测量的协方差矩阵，∑p，q表示点云采集设备坐标系绕惯性坐标系旋转的协方差矩阵。
(6.2)多高度数据处理：该步骤用来处理有多个高程测量值被映射到同一个栅格中的特殊情况。若出现这种情况，分别计算各个高程测量值与已经存储的值之间的马氏距离，选取马氏距离最大的测量值，作为当前栅格的测量值，并忽略其余的测量值。
(6.3)数据融合：若当前栅格内未存储高程测量值，则直接将计算得到的高程测量值
Figure BDA0003272834950000064
加入对应的栅格中；若当前栅格内已经存储有高程测量值
Figure BDA0003272834950000065
则采用卡尔曼滤波，将新计算得到的高程测量值与已储存的进行融合：
Figure BDA0003272834950000066
其中，带有上标+的表示经过融合的新的测量值，带有上标-的表示地图中已存储的测量值。
(6.4)发布更新后的局部高程图。将局部高程图以ROS topic的格式实时发送到系统中，后续节点可以通过订阅的形式获取实时的局部高程图数据。局部高程图的发布帧率设置为每秒10帧。
步骤S107，判断当前点云帧是否为关键帧。关键帧的判断主要以机器人移动的累积位移为标准，机器人运动的累积位移D由下式计算得到：
Figure BDA0003272834950000067
其中，(xi，yi)为上一时刻i由定位系统输出的二维平面定位数据，(xi+1，yi+1)为当前时刻i+1由定位系统输出的二维平面定位数据；N表示当前时刻累积的定位帧数量。当机器人累积位移大于等于阈值时，将当前点云帧判定为关键帧，并进入后续全局高程图的更新步骤。本实施例将累积位移的阈值设置为2m。若判定不是关键帧，则跳到步骤S110。
步骤S108，对局部高程图进行位姿调整。依据步骤S106中已构建的局部高程图，与用于定位的已构建全局点云图，使用点云配准方法寻找两片点云的重叠部分，求出能使两片点云重合程度最大化的位姿变换。具体方法为：将经过体素滤波降采样的局部高程图作为源点云，将用于定位的已构建全局点云图作为目标点云，使用ICP方法进行两块点云的配准，求解使两者之间重合程度最大化的的位姿变换，其中，求解的初始位姿设置为单位矩阵。
进一步地，根据配准结果提供的位姿变换矩阵，将已构建的局部高程图移动到优化后的位置。此时已构建的局部高程图和全局点云图之间的重合程度最大。
步骤S109，更新并发布全局高程图。该步骤引入了全局高程图的概念，将高程图的构建框架改为局部高程图和全局高程图相结合，优点主要是有效消除了局部高程图构建过程带来的地图累计误差。
步骤S109发布的全局高程图与步骤S106发布的局部高程图相比，有以下区别：
1、局部高程图的尺寸大小恒定不变，为5m×5m；而全局高程图的总体尺寸大小不定，会随着构建范围的扩大而增大。
2、局部高程图以机器人坐标系为中心，构建范围随机器人的移动而移动，反映的是机器人周围的局部环境信息；而全局高程图固定在地图坐标系下，不随机器人的移动而移动，反映的是全局的环境信息。
3、局部高程图是实时更新并发布的，发布帧率恒定每秒10帧；而全局高程图由于范围较大，包含信息较多，为确保程序运行的实时性，全局高程图的更新被设置为关键帧条件触发，依据步骤S107中的判断条件触发全局高程图的更新与发布，因此全局高程图的更新与发布是非实时的。
进一步地，全局高程图更新的步骤为：
(9.1)局部高程图加入：将步骤S108中经过位姿调整的局部高程图复制到全局高程图内，由于局部高程图和全局高程图的栅格单元分辨率一致，两者的栅格单元位置也能够一一对应，因此全局高程图的栅格中存储的高程测量值可根据对应的，局部高程图栅格内存储的值进行更新。
(9.2)发布更新后的全局高程图。将全局高程图以ROS topic的格式发送到系统中，后续节点可以通过订阅的形式获取全局高程图数据。局部高程图也可通过ROS service操作保存到本地主机中。
步骤S110：结束整个流程，返回步骤S101，获取下一帧的RGB-D点云。

Claims (7)

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所述基于帧-地图匹配的高程图构建方法，其特征在于，所述更新局部高程图，具体为：
(4.1)计算高程测量值：地形的高程测量值服从高斯分布。地形的高程测量值经过调整后的原输入点云中的每一点在地图坐标系下的z坐标值都代表一个高程测量值的均值，而高程测量值的方差可由以下计算公式得出。
Figure FDA0003272834940000021
其中，Js表示点云采集设备测量的雅可比矩阵；Jq表示点云采集设备坐标系绕地图坐标系旋转的雅可比矩阵；∑s表示点云采集设备测量的协方差矩阵；∑p,q表示点云采集设备坐标系绕惯性坐标系旋转的协方差矩阵；
(4.2)多高度数据处理：若出现有多个高程测量值被映射到同一个栅格中的情况，分别计算各个高程测量值与已经存储的值之间的马氏距离，选取马氏距离最大的测量值，作为当前栅格的测量值，并忽略其余的测量值。
(4.3)数据融合：若当前栅格内未存储高程测量值，则直接将计算得到的高程测量值加入对应的栅格中；若当前栅格内已经存储有高程测量值，则采用卡尔曼滤波，将新计算得到的高程测量值与已储存值进行融合。
6.如权利要求1所述基于帧-地图匹配的高程图构建方法，其特征在于，所述进行全局高程图的处理，具体为：
(5.1)关键帧判断：关键帧的选取以机器人运动的累积位移为标准，机器人运动的累积位移由下式计算得到：
Figure FDA0003272834940000022
其中，(xi,yi)为上一时刻由定位系统输出的二维平面定位数据，(xi+1,yi+1)为当前时刻由定位系统输出的二维平面定位数据。当机器人累计位移大于等于阈值时，将当前点云帧判定为关键帧，继续执行步骤(5.2)；若不是关键帧，则不更新全局高程图。
(5.2)点云配准：将经过体素滤波降采样的局部高程图作为源点云，将用于定位的已构建全局点云图作为目标点云，使用ICP方法进行两块点云的配准，初始位姿设置为单位矩阵，求出两者之间的位姿变换。
(5.3)局部高程图调整：根据有效配准结果提供的位姿变换矩阵，将局部高程图移动到优化后的位置，并复制到全局高程图中。
7.如权利要求1所述基于帧-地图匹配的高程图构建方法，其特征在于，机器人定位方法为使用多线激光雷达的hdl_localization等。
CN202111107037.0A 2021-09-22 2021-09-22 一种基于帧-地图匹配的高程图构建方法 Pending CN113960614A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202111107037.0A CN113960614A (zh) 2021-09-22 2021-09-22 一种基于帧-地图匹配的高程图构建方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202111107037.0A CN113960614A (zh) 2021-09-22 2021-09-22 一种基于帧-地图匹配的高程图构建方法

Publications (1)

Publication Number Publication Date
CN113960614A true CN113960614A (zh) 2022-01-21

Family

ID=79462325

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202111107037.0A Pending CN113960614A (zh) 2021-09-22 2021-09-22 一种基于帧-地图匹配的高程图构建方法

Country Status (1)

Country Link
CN (1) CN113960614A (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116358517A (zh) * 2023-02-24 2023-06-30 杭州宇树科技有限公司 一种用于机器人的高度地图构建方法、系统以及存储介质

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116358517A (zh) * 2023-02-24 2023-06-30 杭州宇树科技有限公司 一种用于机器人的高度地图构建方法、系统以及存储介质
CN116358517B (zh) * 2023-02-24 2024-02-23 杭州宇树科技有限公司 一种用于机器人的高度地图构建方法、系统以及存储介质

Similar Documents

Publication Publication Date Title
CN112347840B (zh) 视觉传感器激光雷达融合无人机定位与建图装置和方法
US20230260151A1 (en) Simultaneous Localization and Mapping Method, Device, System and Storage Medium
EP3520076B1 (en) Computer vision systems and methods for detecting and modeling features of structures in images
KR102145109B1 (ko) 지도 생성 및 운동 객체 위치 결정 방법 및 장치
KR100728377B1 (ko) 레이저 스캐너 및 무선인터넷을 이용한 변경된 지역시설물의 ｇｉｓ 실시간 업데이트 방법
CN110930495A (zh) 基于多无人机协作的icp点云地图融合方法、系统、装置及存储介质
CN111402339B (zh) 一种实时定位方法、装置、系统及存储介质
CN112987065B (zh) 一种融合多传感器的手持式slam装置及其控制方法
CN113168717A (zh) 一种点云匹配方法及装置、导航方法及设备、定位方法、激光雷达
CN113048980B (zh) 位姿优化方法、装置、电子设备及存储介质
WO2020063878A1 (zh) 一种处理数据的方法和装置
CN113820735B (zh) 位置信息的确定方法、位置测量设备、终端及存储介质
KR102127679B1 (ko) 정사영상에 기반한 센서 탑재 이동형 플랫폼의 기하 보정 시스템
CN112967392A (zh) 一种基于多传感器触合的大规模园区建图定位方法
CN111862215B (zh) 一种计算机设备定位方法、装置、计算机设备和存储介质
CN107564046A (zh) 一种基于点云与uav影像二次配准的建筑物轮廓精确提取方法
CN115272596A (zh) 一种面向单调无纹理大场景的多传感器融合slam方法
KR20200028210A (ko) 모바일 맵핑 또는 자율 주행용 플랫폼과 관측 데이터의 구조화를 위한 시스템
Zhang et al. Online ground multitarget geolocation based on 3-D map construction using a UAV platform
CN113960614A (zh) 一种基于帧-地图匹配的高程图构建方法
KR102130687B1 (ko) 다중 센서 플랫폼 간 정보 융합을 위한 시스템
Zhou et al. Visual mapping and localization system based on compact instance-level road markings with spatial uncertainty
CN116817891A (zh) 一种实时多模态感知的高精地图构建方法
CN114842224A (zh) 一种基于地理底图的单目无人机绝对视觉匹配定位方案
Hu et al. Efficient Visual-Inertial navigation with point-plane map

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination