基于3D激光雷达的移动机器人位姿跟踪方法及装置.pdf

《基于3D激光雷达的移动机器人位姿跟踪方法及装置.pdf》由会员分享，可在线阅读，更多相关《基于3D激光雷达的移动机器人位姿跟踪方法及装置.pdf（17页完成版）》请在专利查询网上搜索。

1、(19)国家知识产权局(12)发明专利申请(10)申请公布号 (43)申请公布日 (21)申请号 202410001849.4(22)申请日 2024.01.02(71)申请人 苏州中德睿博智能科技有限公司地址 215347 江苏省苏州市昆山市玉山镇登云路388号1号楼5层(72)发明人 孙波李道胜李涛宋海(74)专利代理机构 湖南兆弘专利事务所(普通合伙)43008专利代理师 胡君(51)Int.Cl.G06T 7/73(2017.01)G06T 7/246(2017.01)G06T 7/33(2017.01)(54)发明名称基于3D激光雷达的移动机器人位姿跟踪方法及装置(57)摘要本发明公。

2、开一种基于3D激光雷达的移动机器人位姿跟踪方法及装置，该方法步骤包括：步骤S01.由搭载有3D激光雷达的移动机器人对所在环境进行遍历扫描，获取激光点云数据；遍历过程中对各帧激光点云数据进行帧间匹配，得到相邻两帧点云数据之间的相对空间关系，根据相对空间关系选取关键帧，保存各个关键帧及位姿；步骤S02.当需要对移动机器人进行跟踪时，实时选取出当前关键帧并进行位姿初始化，从保存的关键帧中选取与初始化后位姿相近的关键帧拼接形成局部子图，利用当前关键帧相对于局部子图的位姿确定出机器人的位姿，实现位姿跟踪。本发明具有实现方法简单、计算复杂度低、跟踪效率以及可靠性高、灵活性强等优点。权利要求书3页 说明书1。

3、0页 附图3页CN 117495968 A2024.02.02CN 117495968 A1.一种基于3D激光雷达的移动机器人位姿跟踪方法，其特征在于，步骤包括：步骤S01.地图构建：由搭载有3D激光雷达的移动机器人对所在环境进行遍历扫描，遍历过程中对扫描得到的各帧激光点云数据进行帧间匹配，依次得到相邻两帧点云数据之间的相对空间关系，根据所述相对空间关系选取关键帧，保存选取的各个关键帧以及关键帧对应的位姿；步骤S02.位姿跟踪：当需要对移动机器人进行跟踪时，在机器人行进过程中，实时从激光雷达扫描得到的激光点云数据中选取出关键帧作为当前关键帧并进行位姿初始化，初始化后从保存的各关键帧中选取出与当。

4、前关键帧位姿相近的关键帧进行拼接形成局部子图，利用当前关键帧相对于所述局部子图的位姿确定出实时机器人的位姿，实现机器人的位姿跟踪。2.根据权利要求1所述的基于3D激光雷达的移动机器人位姿跟踪方法，其特征在于，步骤S01中根据所述相对空间关系选取关键帧时，如果当前帧与上一关键帧之间的平移距离或者旋转角度的累积值大于预设空间阈值，则选取当前帧作为关键帧。3.根据权利要求1所述的基于3D激光雷达的移动机器人位姿跟踪方法，其特征在于，步骤S02包括：步骤S201.每隔指定时间间隔选取一帧激光点云数据作为当前关键帧；步骤S202.对当前关键帧的位姿进行初始化，得到初始化后当前关键帧的位姿；步骤S203.。

5、对当前连续激光数据帧进行帧间匹配得到当前空间变换关系；步骤S204.根据所述当前空间变换关系从保存的各关键帧中查找与初始化后当前关键帧位姿相近的关键帧，将查找到的关键帧拼接形成局部子图；步骤S205.将当前关键帧与局部子图进行匹配得到当前帧相对于局部子图的位姿；步骤S206.将所述当前帧相对于局部子图的位姿施加给局部子图的空间坐标得到当前机器人的位姿；步骤S207.循环执行步骤S201S206，完成机器人位姿跟踪。4.根据权利要求3所述的基于3D激光雷达的移动机器人位姿跟踪方法，其特征在于，步骤S202中，如果当前关键帧激光点云数据为第一帧数据，将当前关键帧对应的位姿初始化为机器人历史轨迹的起。

6、始点位姿，如果当前关键帧激光点云数据为非第一帧数据，将当前关键帧对应的位姿初始化为机器人历史轨迹中与坐标空间距离上最近的关键帧的位姿，其中坐标按照计算得到，为上一时刻关键帧与对应的局部子图之间的相对空间关系，为局部子图的坐标矩阵。5.根据权利要求3所述的基于3D激光雷达的移动机器人位姿跟踪方法，其特征在于，步骤S204中，按照下式将查找到的各关键帧 拼接形成局部子图：其中，为第i个当前关键帧 中的激光点在局部子图中的坐标，为局部子图的坐标，为保存的关键帧 的坐标。6.根据权利要求3所述的基于3D激光雷达的移动机器人位姿跟踪方法，其特征在于，步骤S205中，如果当前关键帧经过位姿初始化后得到的初。

7、始化后当前关键帧的位姿在机器权利要求书1/3 页2CN 117495968 A2人历史轨迹中关键帧附近，关键帧所对应拼接得到的局部子图为，通过ICP配准方法配准当前帧与局部子图得到当前关键帧相对于局部子图的空间坐标关系；步骤S206中，将当前关键帧与局部子图得到当前关键帧相对于局部子图的空间坐标关系施加给局部子图的空间坐标得到当前关键帧的空间坐标，当前关键帧的空间坐标按照下式计算得到：其中，为当前关键帧的空间坐标，即跟踪的机器人位姿，为当前关键帧与局部子图的相对空间关系，为局部子图的空间坐标。7.根据权利要求16中任意一项所述的基于3D激光雷达的移动机器人位姿跟踪方法，其特征在于，所述步骤S0。

8、2中，采用ICP配准方法对扫描得到的激光点云帧数据进行帧间匹配，得到相邻两帧点云数据之间的空间关系，包括：对于两帧连续的激光点云数据和，其中与是两帧激光中相对应距离最小的激光点对，寻找欧式变换T使得两帧激光点云数据满足以下关系式：其中，R为旋转矩阵，t为平移向量，变换矩阵T与旋转矩阵R和平移向量t满足；使用ICP配准方法进行迭代计算直至误差平方和达到极小值，求解得到变换矩阵T。8.一种基于3D激光雷达的移动机器人位姿跟踪装置，其特征在于，包括：地图构建模块，用于由搭载有3D激光雷达的移动机器人对所在环境进行遍历扫描，遍历过程中对扫描得到的各帧激光点云数据进行帧间匹配，依次得到相邻两帧点云数据之。

9、间的相对空间关系，根据所述相对空间关系选取关键帧，保存选取的各个关键帧以及关键帧对应的位姿；位姿跟踪模块，用于当需要对移动机器人进行跟踪时，在机器人行进过程中，实时从激光雷达扫描得到的激光点云数据中选取出关键帧作为当前关键帧并进行位姿初始化，初始化后从保存的各关键帧中选取出与当前关键帧位姿相近的关键帧进行拼接形成局部子图，利用当前关键帧相对于所述局部子图的位姿确定出实时机器人的位姿，实现机器人的位姿跟踪。9.根据权利要求8所述的基于3D激光雷达的移动机器人位姿跟踪装置，其特征在于，所述位姿跟踪模块包括：当前帧选取单元，用于每隔指定时间间隔选取一帧激光点云数据作为当前关键帧，并计算出对应的位姿以。

10、作为被跟踪的机器人位姿；位姿初始化单元，用于对当前关键帧的位姿进行初始化，得到初始化后当前关键帧的位姿；帧间匹配单元，用于对当前连续激光数据帧进行帧间匹配得到当前空间变换关系；局部地图生成单元，用于根据所述当前空间变换关系从保存的各关键帧中查找与初始权利要求书2/3 页3CN 117495968 A3化后当前关键帧位姿相近的关键帧，将查找到的关键帧拼接形成局部子图；帧与子图匹配单元，用于将当前关键帧与局部子图进行匹配得到当前帧相对于局部子图的位姿；位姿转换单元，用于将所述当前帧相对于局部子图的位姿施加给局部子图的空间坐标得到当前机器人的位姿，并将当前机器人的位姿分别传输给所述位姿初始化单元以进。

11、行下一帧位姿初始化以及传输给位姿输出单元以输出当前时刻跟踪到的机器人实时位姿。10.一种基于3D激光雷达的移动机器人位姿跟踪装置，包括3D激光雷达，搭载在移动机器人上，以用于对移动机器人周围环境进行扫描得到激光点云数据，其特征在于，还包括处理器以及存储器，所述存储器用于存储计算机程序，所述处理器用于执行所述计算机程序以执行如权利要求17中任意一项所述方法。权利要求书3/3 页4CN 117495968 A4基于3D激光雷达的移动机器人位姿跟踪方法及装置技术领域0001本发明涉及移动机器人定位技术领域，尤其涉及一种基于3D激光雷达的移动机器人位姿跟踪方法及装置。背景技术0002自主导航功能是移动。

12、机器人智能化程度的重要评价指标，而具有自主导航功能的移动机器人首要解决的问题是确定机器人自身相对周围环境的位姿，即机器人的定位问题。传统的机器人定位方式，如使用全球定位系统（Global Positioning System，GPS）进行导航，必须依赖于GPS信号，移动机器人在树荫遮挡、GPS信号弱等区域内的定位精度和可靠性难以保证，而且在室内、隧道等场景中由于无法接收GPS信号还可能会导致机器人定位完全丢失。定位与建图（simultaneous localization and mapping，SLAM）技术可以解决机器人的定位和地图问题，但现有技术中用于自主导航的SLAM技术大多是基于2D。

13、栅格地图，2D栅格地图是使用二维平面描述三维空间，因此无法准确描述环境中的障碍物，也无法准确反映机器人相对地图的位姿，因而实际定位精度并不高。0003其他如基于机器学习等的机器人定位方法，通过借助深度学习方法识别环境中的语义信息，能够利用语义信息完成机器人的定位，但机器学习方法对机器人搭载计算机的算力具有非常高的要求，并且该类方法的实时性较差，难以提供实时的机器人位姿信息。0004综上，现有技术中针对于自主移动导航机器人定位方法不仅可靠性、实时性较差，而且缺少三维环境的细节描述，难以为自主导航机器人提供稳定准确的位姿信息。发明内容0005本发明要解决的技术问题就在于：针对现有技术存在的技术问题。

14、，本发明提供一种实现方法简单、计算复杂度低、跟踪效率以及可靠性高、灵活性强的基于3D激光雷达的移动机器人位姿跟踪方法及装置，能够实现轻量型移动机器人的准确实时位姿跟踪。0006为解决上述技术问题，本发明提出的技术方案为：一种基于3D激光雷达的移动机器人位姿跟踪方法，步骤包括：步骤S01.地图构建：由搭载有3D激光雷达的移动机器人对所在环境进行遍历扫描，遍历过程中对扫描得到的各帧激光点云数据进行帧间匹配，依次得到相邻两帧点云数据之间的相对空间关系，根据所述相对空间关系选取关键帧，保存选取的各个关键帧以及关键帧对应的位姿；步骤S02.位姿跟踪：当需要对移动机器人进行跟踪时，在机器人行进过程中，实时。

15、从激光雷达扫描得到的激光点云数据中选取出关键帧作为当前关键帧并进行位姿初始化，初始化后从保存的各关键帧中选取出与当前关键帧位姿相近的关键帧进行拼接形成局部子图，利用当前关键帧相对于所述局部子图的位姿确定出实时机器人的位姿，实现机器人的位姿跟踪。0007进一步的，步骤S01中根据所述相对空间关系选取关键帧时，如果当前帧与上一关说明书1/10 页5CN 117495968 A5键帧之间的平移距离或者旋转角度的累积值大于预设空间阈值，则选取当前帧作为关键帧。0008进一步的，步骤S02包括：步骤S201.每隔指定时间间隔选取一帧激光点云数据作为当前关键帧；步骤S202.对当前关键帧的位姿进行初始化，。

16、得到初始化后当前关键帧的位姿；步骤S203.对当前连续激光数据帧进行帧间匹配得到当前空间变换关系；步骤S204.根据所述当前空间变换关系从保存的各关键帧中查找与初始化后当前关键帧位姿相近的关键帧，将查找到的关键帧拼接形成局部子图；步骤S205.将当前关键帧与局部子图进行匹配得到当前帧相对于局部子图的位姿；步骤S206.将所述当前帧相对于局部子图的位姿施加给局部子图的空间坐标得到当前机器人的位姿；步骤S207.循环执行步骤S201S206，完成机器人位姿跟踪。0009进一步的，步骤S202中，如果当前关键帧激光点云数据为第一帧数据，将当前关键帧对应的位姿初始化为机器人历史轨迹的起始点位姿，如果当。

17、前关键帧激光点云数据为非第一帧数据，将当前关键帧对应的位姿初始化为机器人历史轨迹中与坐标空间距离上最近的关键帧的位姿，其中坐标按照计算得到，为上一时刻关键帧与对应的局部子图之间的相对空间关系，为局部子图的坐标矩阵。0010进一步的，步骤S204中，按照下式将查找到的各关键帧 拼接形成局部子图：00110012其中，为第i个当前关键帧 中的激光点在局部子图中的坐标，为局部子图的坐标，为保存的关键帧 的坐标。0013进一步的，步骤S205中，如果当前关键帧经过位姿初始化后得到的初始化后当前关键帧的位姿在机器人历史轨迹中关键帧附近，关键帧所对应拼接得到的局部子图为，通过ICP配准方法配准当前帧与局部。

18、子图得到当前关键帧相对于局部子图的空间坐标关系；步骤S206中，将当前关键帧与局部子图得到当前关键帧相对于局部子图的空间坐标关系施加给局部子图的空间坐标得到当前关键帧的空间坐标，当前关键帧的空间坐标按照下式计算得到：00140015其中，为当前关键帧的空间坐标，即跟踪的机器人位姿，为当前关键帧与局部子图的相对空间关系，为局部子图的空间坐标。0016进一步的，所述步骤S02中，采用ICP（Iterative Closest Point）配准方法对扫描得到的激光点云帧数据进行帧间匹配，得到相邻两帧点云数据之间的空间关系，包括：对于两帧连续的激光点云数据和，其中与是两帧激光中相对应距离最小的激光点对。

19、，寻找欧式变换T使得两帧激光点云数据满足以下关系式：说明书2/10 页6CN 117495968 A600170018其中，R为旋转矩阵，t为平移向量，变换矩阵T与旋转矩阵R和平移向量t满足；0019使用ICP配准方法进行迭代计算直至误差平方和达到极小值，求解得到变换矩阵T。0020一种基于3D激光雷达的移动机器人位姿跟踪装置，包括：地图构建模块，用于由搭载有3D激光雷达的移动机器人对所在环境进行遍历扫描，遍历过程中对扫描得到的各帧激光点云数据进行帧间匹配，依次得到相邻两帧点云数据之间的相对空间关系，根据所述相对空间关系选取关键帧，保存选取的各个关键帧以及关键帧对应的位姿；位姿跟踪模块，用于当。

20、需要对移动机器人进行跟踪时，在机器人行进过程中，实时从激光雷达扫描得到的激光点云数据中选取出关键帧作为当前关键帧并进行位姿初始化，初始化后从保存的各关键帧中选取出与当前关键帧位姿相近的关键帧进行拼接形成局部子图，利用当前关键帧相对于所述局部子图的位姿确定出实时机器人的位姿，实现机器人的位姿跟踪。0021进一步的，所述位姿跟踪模块包括：当前帧选取单元，用于每隔指定时间间隔选取一帧激光点云数据作为当前关键帧，并计算出对应的位姿以作为被跟踪的机器人位姿；位姿初始化单元，用于对当前关键帧的位姿进行初始化，得到初始化后当前关键帧的位姿；帧间匹配单元，用于对当前连续激光数据帧进行帧间匹配得到当前空间变换关。

21、系；局部地图生成单元，用于根据所述当前空间变换关系从保存的各关键帧中查找与初始化后当前关键帧位姿相近的关键帧，将查找到的关键帧拼接形成局部子图；帧与子图匹配单元，用于将当前关键帧与局部子图进行匹配得到当前帧相对于局部子图的位姿；位姿转换单元，用于将所述当前帧相对于局部子图的位姿施加给局部子图的空间坐标得到当前机器人的位姿，并将当前机器人的位姿分别传输给所述位姿初始化单元以进行下一帧位姿初始化以及传输给位姿输出单元以输出当前时刻跟踪到的机器人实时位姿。0022一种基于3D激光雷达的移动机器人位姿跟踪装置，包括3D激光雷达，搭载在移动机器人上，以用于对移动机器人周围环境进行扫描得到激光点云数据，还。

22、包括处理器以及存储器，所述存储器用于存储计算机程序，所述处理器用于执行所述计算机程序以执行如上述方法。0023与现有技术相比，本发明的优点在于：本发明通过先利用3D激光雷达对机器人环境进行感知完成三维环境点云地图的构建，同时通过保留机器人行进过程中筛选出的关键帧，当需要对机器人进行位姿跟踪时，在机器人行进过程中通过查找与实时点云位姿相近的关键帧拼接形成局部子图，利用实时激光点云与局部子图之间的位姿确定出实时机器人说明书3/10 页7CN 117495968 A7的位姿，使得当机器人重新行驶至历史行进轨迹附近时，可以充分利用当前时刻激光雷达观测到的激光点云与保留的坐标和关键帧之间的对应关系完成对。

23、机器人的位姿跟踪，可以大大降低跟踪过程中的计算量以及实现成本，无需依赖于额外的传感器设备，即能够实现移动机器人轻量级的实时、精准位姿跟踪，还可以灵活的应用于各类不同计算能力的移动机器人中。附图说明0024图1是本实施例基于3D激光雷达的移动机器人位姿跟踪方法的实现流程示意图。0025图2是本实施例实现移动机器人位姿跟踪方法的详细流程示意图。0026图3是本实施例实现移动机器人位姿跟踪的原理示意图。具体实施方式0027以下结合说明书附图和具体优选的实施例对本发明作进一步描述，但并不因此而限制本发明的保护范围。0028本发明基于3D激光雷达提供一种轻量型的移动机器人位姿跟踪方法，先利用3D激光雷达。

24、感知机器人周围环境，完成三维环境点云地图的构建，同时通过实时对三维激光点云数据依据相对空间关系选取出关键帧进行保存，当需要对机器人进行位姿跟踪时，在机器人行进过程中，再通过查找是否存在与当前激光点云位姿相近的关键帧，如果查找到则将查找到的各个关键帧拼接形成局部子图，利用当前时刻激光点云与局部子图之间的相对坐标关系以及局部子图的空间坐标确定出下一时刻机器人的位姿，使得当机器人重新行驶至历史行进轨迹附近时，可以充分利用保存的关键帧进行局部子图拼接，进而利用局部子图快速确定机器人相对环境点云地图的位姿，实现机器人相对地图位姿的实时跟踪，从而为移动机器人实现路径规划和自主导航提供准确实时的位姿信息。0。

25、029如图13所示，本实施例基于3D激光雷达的移动机器人位姿跟踪方法的步骤包括：步骤S01.地图构建：由搭载有3D激光雷达的移动机器人对所在环境进行遍历扫描，遍历过程中对扫描得到的各帧激光点云数据进行帧间匹配，依次得到相邻两帧点云数据之间的相对空间关系，根据所述相对空间关系选取关键帧，保存选取的各个关键帧以及关键帧对应的位姿。0030预先将3D激光雷达搭载在移动机器人上，先由移动机器人对所处环境遍历扫描一周，以实现对待跟踪目标区域内对周围环境的感知，完成三维环境点云地图的构建，在扫描过程中获取扫描得到的3D激光点云数据。在遍历扫描过程中，对扫描得到的各帧激光点云数据进行帧间匹配，依次得到相邻两。

26、帧点云数据之间的相对空间关系。对3D激光雷达扫描得到的激光点云数据帧，通过ICP方法配准两帧激光点云得到两帧激光点云之间的相对空间关系，即机器人在两时刻之间的运动状态估计。0031具体地，假设两帧连续的激光点云数据和，其中与是两帧激光中相对应距离最小的激光点对，寻找欧式变换T使得两帧激光点云数据满足以下关系式：0032（1）说明书4/10 页8CN 117495968 A80033其中，R为旋转矩阵，t为平移向量，变换矩阵T与旋转矩阵R和平移向量t满足：（2）0034使用ICP配准方法进行迭代计算直至按照下式计算的误差平方和达到极小值，求解得到变换矩阵T；（3）0035根据各相邻两帧点云数据之。

27、间的相对空间关系选取关键帧，保存选取的各个关键帧点云以及关键帧对应的位姿。对3D激光雷达扫描得到的激光点云数据，通过连续的两帧激光点云通过配准后获得了两帧激光点云之间的相对空间关系，即相对运动关系，进一步根据各相邻两帧点云数据之间的相对空间关系选取特定的激光帧关键帧。0036具体地，如果当前帧与上一关键帧之间的平移距离或者旋转角度的累积值大于预设空间阈值，则选取当前帧作为关键帧。可以理解的是，也可以根据实际需求采用除平移距离、旋转角度以外的其他空间关系参数选取各关键帧。0037对于选取出的关键帧，保留每个关键帧的激光点云数据以及位姿，非关键帧的普通激光帧则只参与运动推算不保留激光点云，将关键帧。

28、以及关键帧的位姿进行保存以用于后续位姿跟踪。至此即得到了机器人行进轨迹上间断离散的坐标点以及每个坐标点上激光雷达扫描到的周围环境激光点云，通过先保留这些离散的坐标和每个坐标观测到的关键帧，当机器人重新遍历之前行进轨迹附近时，可以利用当前时刻激光雷达观测到的激光点云与保留的坐标和关键帧之间的对应关系完成对机器人的位姿跟踪，可以大大降低跟踪过程中的计算量、减少数据存储量，提高跟踪效率。0038步骤S02.当需要对移动机器人进行跟踪时，在机器人行进过程中，实时从激光雷达扫描得到的激光点云数据中选取出关键帧作为当前关键帧并进行位姿初始化，初始化后从保存的各关键帧中选取出与当前关键帧位姿相近的关键帧进行。

29、拼接形成局部子图，利用当前关键帧相对于所述局部子图的位姿确定出实时机器人的位姿，实现机器人的位姿跟踪。0039步骤S201.每隔指定时间间隔选取一帧激光点云数据作为当前关键帧。0040持续的输出机器人的位姿信息对计算机算力是一种不必要的浪费，本实施例通过设定时间阈值，当需要跟踪机器人位姿时，每隔固定的时间间隔选取一帧激光点云作为当前激光帧，并计算该激光帧相对机器人环境的位姿，输出该激光帧的位姿作为被跟踪的机器人位姿，可以确保跟踪精度的同时，进一步降低所需的计算量。0041步骤S202.对当前帧的位姿进行初始化，得到初始化后当前关键帧的位姿。0042选取出当前关键帧后，需要对当前关键帧的位姿进行。

30、初始化。具体地，如果当前关键帧激光点云数据为第一帧数据，将当前关键帧对应的位姿初始化为机器人历史轨迹的起始点位姿，如果当前关键帧激光点云数据为非第一帧数据，将当前关键帧对应的位姿初始化为机器人历史轨迹中与坐标空间距离上最近的关键帧的位姿。即位姿初始化分两种工作状态，状态一对应如果当前关键帧为第一帧数据时，将当前帧位姿初始化为机器人历史轨迹的起始点位姿，状态二对应如果当前关键帧非第一帧数据时，将当前帧位姿初始化为机器人历史轨迹的某一关键帧位姿，即保存的某一关键帧的位姿。说明书5/10 页9CN 117495968 A90043在具体应用实施例中，坐标具体按照下式计算得到：0044（4）0045其。

31、中，为上一时刻激光点云关键帧与对应的局部子图之间的相对空间关系，为局部地图的坐标矩阵。0046利用上一时刻激光点云关键帧与对应的局部地图之间的相对空间关系以及局部地图的坐标计算出坐标后，通过查找保存关键帧的数据库中与坐标空间距离上最近的机器人历史轨迹的关键帧位姿，并将查找到的该关键帧位姿赋值给当前激光帧完成当前激光点云关键帧的位姿初始化。0047步骤S203.对当前连续激光数据帧进行帧间匹配得到当前空间变换关系。0048具体地，通过ICP配准方法完成连续激光帧之间的运动状态估计，直到依据时间间隔选出下一帧，通过帧间匹配完成位姿初始化后的当前帧与选取的下一个激光帧之间的空间变换关系。0049步骤。

32、S204.根据当前空间变换关系从保存的各关键帧中查找与初始化后当前关键帧位姿相近（空间距离在预设阈值以内）的关键帧，将查找到的关键帧拼接形成局部子图（局部地图）。0050使用保存的关键帧位姿拼接保存的部分关键帧形成一个子图。假设当前激光帧经过位姿初始化模块进行位姿初始化后的位姿与关键帧的位姿相近，则通过拼接关键帧空间距离满足阈值的若干关键帧构建局部子图。0051具体地，设定局部子图的坐标为关键帧对应的位姿，按照下式将各关键帧拼接形成局部子图：0052（5）0053其中，为第i个激光数据帧 中的激光点在局部子图中的坐标表达，为局部子图的坐标，为保存的激光数据帧 的坐标。0054步骤S205.将当。

33、前关键帧与局部子图进行匹配得到当前关键帧相对于局部子图的位姿。0055具体地，如果当前帧经过位姿初始化后得到的初始化后当前关键帧的位姿在保存的机器人历史轨迹中关键帧附近，关键帧所对应拼接得到的局部子图（局部地图）为，本实施例通过ICP配准方法配准当前帧与局部子图得到当前帧相对于局部子图的空间坐标关系，并输出空间坐标关系以用于进行位姿转换。0056步骤S206.将当前帧相对于局部子图的位姿施加给局部子图的空间坐标得到当前机器人的位姿，完成机器人位姿跟踪。0057具体地，将当前帧相对于局部子图的空间坐标关系施加给局部子图的空间坐标得到当前机器人的位姿，完成机器人位姿跟踪。0058在具体应用实施例中。

34、，当前关键帧的空间坐标按照下式计算得到：0059（6）说明书6/10 页10CN 117495968 A100060其中，为当前关键帧的空间坐标，即跟踪的机器人位姿，为当前关键帧与局部子图的相对空间关系，为局部子图的空间坐标。0061将得到的当前关键帧的空间坐标分两路输出，一路用于下一帧的位姿初始化，另一路将当前帧的空间坐标实时发布以作为被跟踪机器人的实时位姿。0062步骤S207.循环执行步骤S201S206，完成机器人位姿跟踪。0063本实施例通过先利用3D激光雷达感知机器人周围环境，在遍历扫描过程中利用3D激光点云数据处理保存多个关键帧，完成机器人所处三维环境的地图建模，在机器人实时跟踪。

35、过程中利用保存的关键帧确定机器人相对环境点云地图的位姿，对机器人位姿进行跟踪完成机器人的实时定位，为机器人实现自主导航提供准确的定位信息，只需要借助于一个3D激光雷达即可完成环境建模和机器人位姿跟踪，无需再融合额外的机器人传感器，可以大大降低计算量，实现轻量的位姿跟踪，还可以方便部署在不同算力的移动机器人平台上，适应各种移动机器人在不同工作场景中的定位需求。0064如图2所示，本实施例基于3D激光雷达的移动机器人位姿跟踪装置包括：地图构建模块，用于由搭载有3D激光雷达的移动机器人对所在环境进行遍历扫描，遍历过程中对扫描得到的各帧激光点云数据进行帧间匹配，依次得到相邻两帧点云数据之间的相对空间关。

36、系，根据相对空间关系选取关键帧，保存选取的各个关键帧以及关键帧对应的位姿；位姿跟踪模块，用于当需要对移动机器人进行跟踪时，在机器人行进过程中，实时从激光雷达扫描得到的激光点云数据中选取出关键帧作为当前关键帧并进行位姿初始化，初始化后从保存的各关键帧中选取出与当前关键帧位姿相近的关键帧进行拼接形成局部子图，利用当前关键帧相对于所述局部子图的位姿确定出实时机器人的位姿，实现机器人的位姿跟踪。0065本实施例中，位姿跟踪模块具体包括：当前帧选取单元，用于每隔指定时间间隔选取一帧激光点云数据作为当前关键帧，并计算出对应的位姿以作为被跟踪的机器人位姿；位姿初始化单元，用于对当前关键帧的位姿进行初始化，得。

37、到初始化后当前关键帧的位姿；帧间匹配单元，用于对当前连续激光数据帧进行帧间匹配得到当前空间变换关系；局部地图生成单元，用于根据所述当前空间变换关系从保存的各关键帧中查找与初始化后当前关键帧位姿相近的关键帧，将查找到的关键帧拼接形成局部子图；帧与子图匹配单元，用于将当前关键帧与局部子图进行匹配得到当前帧相对于局部子图的位姿；位姿转换单元，用于将所述当前帧相对于局部子图的位姿施加给局部子图的空间坐标得到当前机器人的位姿，并将当前机器人的位姿分别传输给所述位姿初始化单元以进行下一帧位姿初始化以及传输给位姿输出单元以输出当前时刻跟踪到的机器人实时位姿。0066在具体应用实施例中，如图2、3所示，先由搭。

38、载有3D激光雷达的移动机器人遍历环境一周，在此期间通过第一帧间匹配单元采用ICP方法配准连续的激光点云得到相邻激光帧之间的相对空间关系，由第一关键帧选取单元依据相对空间关系选取部分激光帧作为关说明书7/10 页11CN 117495968 A11键帧，并保存各个关键帧的激光点云，输出给里程计模块以变换矩阵的格式保存每个关键帧的空间坐标。0067机器人遍历环境一周后，由第一关键帧选取单元和里程计模块分别存储每个关键帧的点云和空间坐标在计算机本地硬盘。当需要实时跟踪机器人的位姿时，将机器人在遍历过的场景中开机运行，然后机器人从计算机本地硬盘读取遍历该场地保存的关键帧点云和关键帧对应的空间坐标。由位。

39、姿跟踪模块中第二关键帧选取单元依据时间阈值选取连续激光帧的某一帧作为当前激光帧，由第二帧间匹配单元通过帧间匹配方法得到当前连续关键帧之间的相对空间关系，局部地图生成单元从计算机本地硬盘中读取与当前激光帧空间坐标邻近的激光帧然后由若干个邻近帧拼接成一个局部子图，进而通过帧与子图匹配单元配准当前关键帧与局部子图得到当前关键帧相对局部地图的空间坐标，最后由位姿转换单元将当前关键帧相对局部地图的空间关系转换为当前帧的空间坐标，分别传输给位姿初始化单元以用于初始化下一激光帧的初始位姿，以及输出给位姿输出模块以作为跟踪到的机器人实时位姿。0068在具体应用实施例中，利用上述移动机器人位姿跟踪装置实现移动机。

40、器人位姿跟踪的详细步骤为：步骤1：输入模块通过机器人搭载的3D激光雷达感知机器人周围的环境，并将扫描到的激光点云帧传输给配准模块。0069步骤2：在机器人扫描过程中，由配准模块通过ICP方法配准两帧激光点云得到两帧激光点云之间的相对空间关系，即机器人在两时刻之间的运动状态估计，其中对于两帧连续的激光点云和，其中与是两帧激光中相对应距离最小的激光点对，通过查找欧式变换T，使得两帧激光点云尽可能的重叠，即两帧点云满足关系式：，然后采样ICP方法通过迭代计算误差平方和达到极小值后解得变换矩阵T，即得到两帧激光点云之间的相对运动关系。0070步骤3：在机器人扫描过程中，由关键帧选取模块依据空间变换关系。

41、选取特定的激光帧作为关键帧，其中如果当前帧与上一关键帧之间的平移距离或者旋转角度的累积值大于设定的空间阈值后则选取当前帧作为关键帧，关键帧选取模块保留每个关键帧的激光点云，普通激光帧只参与运动推算不保留激光点，并将关键帧的位姿传输给里程计模块。里程计模块接收关键帧模块传输的各个关键帧的位姿，并将它们以变换矩阵的格式储存在计算机本地文本文档中。0071步骤4：启动位姿跟踪模块，由当前帧选取单元依据时间关系选取特定的激光帧作为当前激光帧，截图每隔固定的时间间隔选取一帧激光点云作为当前激光帧，并计算该激光帧相对机器人环境的位姿，输出该激光帧的位姿作为被跟踪的机器人位姿。0072步骤5：由位姿初始化单。

42、元分两种工作状态对当前帧进行初始化，如果当前激光帧为第一帧激光时，位姿初始化模块将当前帧位姿初始化为机器人历史轨迹的起始点位姿，如果当前激光帧非第一激光帧时，位姿初始化模块将当前激光帧位姿初始化为机器人历史轨迹的某一关键帧位姿。位姿初始化模块具体通过查找计算机本地文本文档中与坐标空间距离上最近的机器人历史轨迹的关键帧位姿，并将该关键帧位姿赋值给当前激光帧完成当前激光帧的位姿初始化。说明书8/10 页12CN 117495968 A120073步骤6：帧间匹配单元通过ICP方法完成连续激光帧之间的运动状态估计，直到当前帧选取单元依据时间间隔选出下一激光帧，通过帧间匹配单元计算位姿初始化后的当前激。

43、光帧与当前帧选取单元选取的下一个激光帧之间的空间变换关系。0074步骤7：局部地图生成单元使用计算机本地文本文档保存的关键帧位姿拼接由关键帧选取模块保存的部分关键帧形成一个子图，其中假设当前激光帧经过位姿初始化模块进行位姿初始化后的位姿与关键帧的位姿相同，局部地图生成单元通过拼接关键帧空间距离满足阈值的若干关键帧构建局部地图，用于拼接局部地图的其它关键帧中的激光点满足对应关系：，其中，为激光帧中的激光点在局部地图中的坐标表达，为局部地图的坐标，为里程计模块保存的激光帧 的坐标。0075步骤8：帧与子图匹配单元通过ICP方法配准当前帧与局部地图从而获得当前帧相对于局部地图的位姿，如果当前帧经过位。

44、姿初始化后在机器人历史轨迹中的某一关键帧附近，由局部地图模块拼接的关键帧所对应的局部地图为，子图匹配模块通过ICP方法配准当前帧与局部地图得到当前帧相对于局部地图的空间坐标关系，并将空间坐标关系传输给位姿转换单元。0076步骤9：位姿转换单元将帧与帧与子图匹配单元得到的当前帧相对于局部地图的空间坐标关系施加给局部地图的空间坐标得到当前机器人的位姿，完成机器人位姿跟踪。位姿转换单元得到的当前帧的空间坐标分别传输给位姿初始化模块用于位姿初始化和位姿输出模块作为当前时刻跟踪到的机器人实时位姿。位姿输出模块接收位姿转换模块得到的当前帧的空间坐标并实时发布该坐标作为被跟踪机器人的实时位姿完成机器人位姿的。

45、实时跟踪。0077本实施例仅需要使用一个3D激光雷达就可以完成对机器人位姿的实时跟踪，为需要自主导航的移动机器人提供实时准确的定位和环境地图信息，不仅所需使用传感器少、成本低，且计算简单、计算量小，可以实现轻量化跟踪计算，对机器人搭载的计算机算力要求低，可以方便的部署在不同的移动机器人平台上，即便是应用于算力低的移动机器人平台，仍然可以实现实时的机器人位姿跟踪，同时还便于进行升级优化，以适用于各类不同场景中移动机器人实现实时定位的自主导航。0078本实施例还提供一种基于3D激光雷达的移动机器人位姿跟踪装置，包括3D激光雷达，搭载在移动机器人上，以用于对移动机器人周围环境进行扫描得到激光点云数据。

46、，还包括处理器以及存储器，存储器用于存储计算机程序，处理器用于执行计算机程序以执行如上述方法。0079可以理解的是，本实施例上述方法可以由单个设备执行，例如一台计算机或服务器等，也可以应用于分布式场景下由多台设备相互配合来完成，在分布式场景的情况下，多台设备中的一台设备可以只执行本实施例上述方法中的某一个或多个步骤，多台设备之间进行交互以完成上述方法。处理器可以采用通用的CPU、微处理器、应用专用集成电路、或者一个或多个集成电路等方式实现，用于执行相关程序，以实现本实施例上述方法。存储器可以采用只读存储器ROM、随机存取存储器RAM、静态存储设备以及动态存储设备等形式实现。存储器可以存储操作系。

47、统和其他应用程序，在通过软件或者固件来实现本实施例上述方法说明书9/10 页13CN 117495968 A13时，相关的程序代码保存在存储器中，并由处理器来调用执行。0080上述只是本发明的较佳实施例，并非对本发明作任何形式上的限制。虽然本发明已以较佳实施例揭露如上，然而并非用以限定本发明。因此，凡是未脱离本发明技术方案的内容，依据本发明技术实质对以上实施例所做的任何简单修改、等同变化及修饰，均应落在本发明技术方案保护的范围内。说明书10/10 页14CN 117495968 A14图1说明书附图1/3 页15CN 117495968 A15图2说明书附图2/3 页16CN 117495968 A16图3说明书附图3/3 页17CN 117495968 A17。