CN112729252B - 基于机器人平台的隧道激光点云采集方法及机器人系统 - Google Patents

基于机器人平台的隧道激光点云采集方法及机器人系统 Download PDF

Info

Publication number
CN112729252B
CN112729252B CN202011501913.3A CN202011501913A CN112729252B CN 112729252 B CN112729252 B CN 112729252B CN 202011501913 A CN202011501913 A CN 202011501913A CN 112729252 B CN112729252 B CN 112729252B
Authority
CN
China
Prior art keywords
current
tunnel
point cloud
laser
robot
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.)
Active
Application number
CN202011501913.3A
Other languages
English (en)
Other versions
CN112729252A (zh
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.)
Shenzhen Research Center Of Digital City Engineering
Shenzhen University
Original Assignee
Shenzhen Research Center Of Digital City Engineering
Shenzhen University
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 Shenzhen Research Center Of Digital City Engineering, Shenzhen University filed Critical Shenzhen Research Center Of Digital City Engineering
Priority to CN202011501913.3A priority Critical patent/CN112729252B/zh
Publication of CN112729252A publication Critical patent/CN112729252A/zh
Application granted granted Critical
Publication of CN112729252B publication Critical patent/CN112729252B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C11/00—Photogrammetry or videogrammetry, e.g. stereogrammetry; Photographic surveying
    • G01C11/04—Interpretation of pictures
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20—Instruments for performing navigational calculations

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Multimedia (AREA)
  • Length Measuring Devices By Optical Means (AREA)

Abstract

本发明所提供的基于机器人平台的隧道激光点云采集方法及机器人系统，所述方法包括：获取当前第一位置、当前第一姿态及隧道内预设控制点，确定当前第一激光点云；根据所述预设控制点纠正所述当前第一位置和所述当前第一姿态，得到当前第二位置和当前第二姿态；建立所述当前第二位置、所述当前第二姿态与预设路线之间的匹配关系，确定导航路线；根据所述当前第二位置、所述当前第二姿态和所述导航路线，确定隧道三维点云。本发明能够得到高精度的隧道三维点云，且节省了人工测量环节，提高了测量精度、效率和成本。

Description

基于机器人平台的隧道激光点云采集方法及机器人系统
技术领域
本发明涉及计算机技术领域，尤其涉及的是基于机器人平台的隧道激光点云采集方法及机器人系统。
背景技术
在隧道交通设计、施工及运营过程中，为保证隧道交通设计、施工及运营的安全合理性，需要对隧道进行多次分阶段的精密测量，以获取隧道的测绘和监测数据，便于在隧道施工设计阶段根据实际施工情况与规划设计对比，以进行调线调坡；在隧道竣工阶段，需要与设计好的BIM模型进行比较，以作为工程验收的标准；在隧道运营阶段，对隧道进行维护保证其正常使用。
传统的测量方式是专业人员通过测量工具对隧道进行测量，并对测量得到的数据进行分析，进而评估隧道的安全性和需要改进的方面。
但由于隧道的特殊性和作业环境的复杂性，通过传统的测量方式则存在测量精度低、测量周期长、测量效率低以及不能保障测量人员自身安全的问题。
因此，现有技术存在缺陷，有待改进与发展。
发明内容
本发明要解决的技术问题在于，针对现有技术的上述缺陷，提供基于机器人平台的隧道激光点云采集方法及机器人系统，旨在解决现有技术中的对隧道进行测量存在测量精度低、测量周期长、测量效率低以及不能保障测量人员自身安全的问题。
本发明解决技术问题所采用的技术方案如下：
获取当前第一位置、当前第一姿态及隧道内预设控制点，确定当前第一激光点云；
根据所述预设控制点纠正所述当前第一位置和所述当前第一姿态，得到当前第二位置和当前第二姿态；
建立所述当前第二位置、所述当前第二姿态与预设路线之间的匹配关系，确定导航路线；
根据所述当前第二位置、所述当前第二姿态和所述导航路线，确定隧道三维点云。
进一步地，所述建立所述当前第二位置、所述当前第二姿态与预设路线之间的匹配关系，确定导航路线包括：
通过地图匹配算法，建立所述当前第二位置与所述预设路线的匹配关系；
根据所述匹配关系确定所述当前第二位置和所述当前第二姿态与所述预设路线之间的偏差参数；
按照所述偏差参数调整行进过程中的行进参数，得到所述导航路线。
进一步地，所述根据所述预设控制点纠正所述当前第一位置和所述当前第一姿态，得到当前第二位置和当前第二姿态包括：
对比所述预设控制点的观测坐标与多个预先实测得到的控制点的实际坐标，确定观测所述预设控制点的观测误差；
根据所述观测误差对所述当前第一位置和所述当前一姿态进行纠正，得到所述当前第二位置和所述当前第二姿态。
进一步地，所述根据所述观测误差对所述当前第一位置和所述当前一姿态进行纠正，得到所述当前第二位置和所述当前第二姿态包括：
获取行进过程中的位置增量，根据所述位置增量确定所述当前第一位置和所述当前第一姿态的惯性残差；
将所述惯性残差和所述观测误差作为扩展卡尔曼滤波模型的输入，输出对所述当前第一位置和所述当前第一姿态的误差修正量；
根据所述误差修正量纠正所述当前第一位置和所述当前第一姿态，分别得到所述当前第二位置和所述当前第二姿态。
进一步地，所述根据所述误差修正量纠正所述当前第一位置和所述当前第一姿态，分别得到所述当前第二位置和所述当前第二姿态，之后还包括：
根据所有的当前第二位置和所有的当前第二姿态确定纠正后的第一行进轨迹；
通过RIS平滑算法对所述第一行进轨迹进行反向平滑处理，之后进行内插，得到第二行进轨迹。
进一步地，所述根据所述当前第二位置、所述当前第二姿态和所述导航路线，确定隧道三维点云包括：
对每个对应的所述当前第一激光点云、所述当前第二位置和所述当前第二姿态分别进行坐标系转换，在测图坐标系中融合解算得到当前第二激光点云；
根据所述第二行进轨迹和所有当前第二激光点云，得到隧道三维点云。
进一步地，所述根据所述当前第二位置、所述当前第二姿态和所述导航路线，确定隧道三维点云还包括：
当存在多段所述第二行进轨迹时，分别对每段所述第二行进轨迹中的所述当前第二位置、所述当前第二姿态和所述当前第一激光点云进行坐标系转换，在测图坐标系中解算得到多段所述第二行进轨迹对应的所述当前第二激光点云；
将多段所述第二行进轨迹中的所述当前第二激光点云根据所述第二行进轨迹分别进行初步拼接，得到隧道的多段初始激光点云。
进一步地，所述将多段所述第二行进轨迹中的所述当前第二激光点云根据所述第二行进轨迹分别进行初步拼接，得到隧道的多段初始激光点云，之后还包括：
提取多段所述初始激光点云中重合的点云进行配准；
对配准后的多段所述初始激光点云进行拼接，得到完整的隧道三维点云。
进一步地，所述获取当前第一位置、当前第一姿态及隧道内预设控制点，确定当前第一激光点云，之前还包括：
通过双目立体相机获取隧道内预设控制点的图像；
根据与所述双目立体相机的标定关系以及所拍摄图像的立体像对的后方交会，确定初始位置和初始姿态。
本发明还提供一种机器人系统，其中，所述机器人系统包括：存储器、处理器及存储在所述存储器上并可在所述处理器上运行的基于机器人系统的隧道激光点云采集程序，所述基于机器人系统的隧道激光点云采集程序被所述处理器执行时实现如上所述的基于机器人平台的隧道激光点云采集方法的步骤。
本发明所提供的基于机器人平台的隧道激光点云采集方法及机器人系统，所述方法包括：获取当前第一位置、当前第一姿态及隧道内预设控制点，确定当前第一激光点云；根据所述预设控制点纠正所述当前第一位置和所述当前第一姿态，得到当前第二位置和当前第二姿态；建立所述当前第二位置、所述当前第二姿态与预设路线之间的匹配关系，确定导航路线；根据所述当前第二位置、所述当前第二姿态和所述导航路线，确定隧道三维点云。本发明能够得到高精度的隧道三维点云，且节省了人工测量环节，提高了测量精度、效率和成本。
附图说明
图1是本发明中基于机器人平台的隧道激光点云采集方法的较佳实施例的流程图。
图2是本发明基于机器人平台的隧道激光点云采集方法的较佳实施例中步骤S100的流程图；
图3是本发明基于机器人平台的隧道激光点云采集方法的较佳实施例中步骤S300的流程图；
图4是本发明基于机器人平台的隧道激光点云采集方法的较佳实施例中机器人实现自主导航的示意图；
图5是本发明基于机器人平台的隧道激光点云采集方法的较佳实施例中步骤S200的流程图；
图6是本发明基于机器人平台的隧道激光点云采集方法的较佳实施例中步骤S220的流程图；
图7是本发明基于机器人平台的隧道激光点云采集方法的较佳实施例中对当前位置和当前姿态进行纠正示意图；
图8是本发明基于机器人平台的隧道激光点云采集方法的较佳实施例中步骤S400的流程图；
图9是本发明基于机器人平台的隧道激光点云采集方法的另一较佳实施例中当分段计算点云时，步骤S400的流程图；
图10是本发明中机器人系统的较佳实施例的功能原理框图。
具体实施方式
为使本发明的目的、技术方案及优点更加清楚、明确，以下参照附图并举实施例对本发明进一步详细说明。应当理解，此处所描述的具体实施例仅用以解释本发明，并不用于限定本发明。
目前隧道测量主要使用的是全站仪、水准仪及激光测距仪等设备，且需要人工进入隧道使用这些设备进行测量，存在作作业效率低、作业周期长、测量精度低且不能保障测量人员安全的问题。而且对于隧道的变形、渗水、裂缝等病害，即使采用点状测量的方式，有时也很难描述隧道整体特征。
本发明基于机器人系统进行测量，一方面通过机器人代替人工进行测量，提高了测量效率，避免了人工测量的安全隐患；另一方面通过更高精度的激光器进行测量，极大提高了隧道数据的覆盖面，且能够获取到隧道的三维点云模型，再者激光的空间位置和强度能够更加适应隧道的变形、渗水、裂缝等病害，提高了测量的精确度。另外，通过机器人搭载激光器在隧道中移动测量，能够大幅度提升检测效率和可靠性、减少人工劳动强度、提高检测的准确率和精度。
本发明通过设计机器人在隧道中自主导航行走，同时机器人上搭载了激光器对隧道进行扫描获得测量数据，之后结合机器人与扫描器之间的关系，得到隧道的三维点云。而机器人实现自动对数据的采集与处理，进而呈现处理结果，也可与别的平台进行远程通讯，实现数据采集、处理及结果的实时共享。
请参见图1，图1是本发明中一种基于机器人平台的隧道激光点云采集方法及机器人系统的流程图。如图1所示，本发明实施例所述的一种基于机器人平台的隧道激光点云采集方法及机器人系统包括以下步骤：
步骤S100、获取当前第一位置、当前第一姿态及隧道内预设控制点，确定当前第一激光点云。
具体地，机器人上搭载有高精度惯性导航单元(IMU)，用于获取机器人的当前第一位置和当前第一姿态，同时搭载有里程计，用于获取机器人行进中的位置增量。且隧道中预先设置有控制点，其中，控制点通常为CPIII(控制网布设)或人工布设靶标，且控制点的中心坐标提前通过全站仪测量得到。当机器人在隧道中自主导航行走时，机器人则控制其上设置的激光扫描仪进行激光测距，进而可根据当前第一位置、当前第一姿态及隧道内预设控制点对激光测距进行处理，得到当前第一激光点云。
在进一步具体实施例中，当机器人启动时，并不能根据惯性导航单元和里程计获取位置和姿态信息，则可通过设置于机器人上的双目立体相机获取初始位置和初始姿态，如图2所示，步骤S100包括：
步骤S110、通过双目立体相机获取隧道内预设控制点的图像。
具体地，机器人上配置有标定好的双目立体相机，通过双目立体相机即可拍摄隧道中预先设置的控制点图像，进而通过对图像进行分析，能够得到控制点的坐标。
步骤S120、根据与所述双目立体相机的标定关系以及所拍摄图像的立体像对的后方交会，确定初始位置和初始姿态。
具体地，通过立体像对的后方交会技术对双目立体相机拍摄的同一个控制点的两张图像进行处理，即可确定双目立体相机的位置和姿态，进而通过双目立体相机与机器人的标定关系，即可确定机器人启动时的初始位置和初始姿态。
步骤S200、根据所述预设控制点纠正所述当前第一位置和所述当前第一姿态，得到当前第二位置和当前第二姿态。
具体地，预设控制点的坐标是预先测量好的标准坐标，以预设控制点坐标为依据即可实现对机器人实际移动过程中的当前第一位置和当前第一姿态进行纠正，进而得到准确的当前第二位置和当前第二姿态。
步骤S300、建立所述当前第二位置、所述当前第二姿态与预设路线之间的匹配关系，确定导航路线。
具体地，预设路线即为设计隧道时的设计路线，但隧道的实际情况则与设计路线存在偏差，因此，在机器人行进过程中可以设计路线作为参照，进而当前第二位置和当前第二姿态确定导航路线，实现机器人在隧道中自主导航。
步骤S400、根据所述当前第二位置、所述当前第二姿态和所述导航路线，确定隧道三维点云。
当机器人在隧道中行进过程中，则控制搭载在机器人上的激光器扫描采集隧道的点云数据，在行进过程中通过激光器进行激光测距以解算出初始激光点云，之后通过当前第二位置和当前第二姿态对初始激光点云进行修正，得到高精度的隧道激光点云。其中，激光器可以为高精度激光扫描仪，能够大幅度提高扫描覆盖面和扫描效率，也提高了扫描的精准度。
在一具体实施例中，如图3所示，步骤S300包括：
步骤S310、通过地图匹配算法，建立所述当前第二位置与所述预设路线的匹配关系。
具体地，对机器人的当前第二位置与预设路线进行处理时，采用地图匹配算法，能够减小当前第二位置与预设路线匹配时的误差，进而建立两者之间的匹配关系。
步骤S320、根据所述匹配关系确定所述当前第二位置和所述当前第二姿态与所述预设路线之间的偏差参数。
具体地，根据当前第二位置和当前第二姿态即可确定当前坐标点，将该坐标点与预设路线上对应位置点的坐标进行比对，即可确定当前行进路径的角度偏差，通过机器人系统坐标系和预设路线坐标系之间的关系可以调整机器人的行进方向。
步骤S330、按照所述偏差参数调整行进过程中的行进参数，得到所述导航路线。
通过偏差参数对机器人的位置、姿态和速度等参数进行修正，即可确定机器人的导航路线，为机器人在隧道中行走提供依据。
参见图4，机器人在隧道中通过立体相机和控制点确定自身初始位置和初始姿态，之后在行进过程中通过惯性导航单元和里程计获取当前第一位置和当前第一姿态，进而通过控制点实际坐标对当前第一位置和当前第一姿态进行纠正，实现机器人对自身的定位，从而建立当前第二位置与预设路线之间的匹配关系之后，根据预设路线实现在隧道中的自主导航。
在一具体实施例中，如图5所示，步骤S200包括：
步骤S210、对比所述预设控制点的观测坐标与多个预先实测得到的控制点的实际坐标，确定观测所述预设控制点的观测误差。
具体地，机器人系统对自身的位置和姿态的定位存在误差，则需要通过控制点的实际坐标确定机器人的观测误差。其中，对于控制点的观测而言，可将通过机器人的观测坐标与实际坐标进行对比，进而确定观测误差。控制点的实际坐标通过激光扫描仪或定位激光雷达测量得到，其中，定位激光雷达还可以用于实现避障功能，根据实际坐标与观测坐标之间的偏差，即可确定机器人观测控制点的观测误差。
步骤S220、根据所述观测误差对所述当前第一位置和所述当前一姿态进行纠正，得到所述当前第二位置和所述当前第二姿态。
具体地，根据观测坐标的位置确定同一个观测点对应观测坐标的偏差程度，即为偏差参数，所述偏差参数包括位置偏差、姿态偏差和速度偏差。进而通过偏差参数对当前第一位置和当前第一姿态进行纠正，即可得到当前第二位置和当前第二姿态。
在一具体实施例中，如图6和图7所示，步骤S220包括：
步骤S221、获取行进过程中的位置增量，根据所述位置增量确定所述当前第一位置和所述当前第一姿态的惯性残差。
具体地，机器人行驶轮上设置有里程计，行驶轮进行同轴运动，可在每个行驶轮上均设置一个里程计，可以获得机器人行进过程中的位置增量。机器人上还设置有高精度惯性导航单元，能够获取机器人的当前第一位置和当前第一姿态。
其中，惯性残差表示为里程计的位置误差和惯性导航单元的位置误差和姿态误差，实现利用里程计的测得的里程约束惯性导航单元的累计误差，减小了行进过程中的测量误差。
进一步地，可通过惯性残差更新惯性导航单元测量的数据，并结合里程计测量的位置增量循环输入卡尔曼滤波模型中实现惯性残差的更新与计算。
具体地，通过根据位置增量对当前第一位置和当前第一姿态分别计算惯性残差，进而将惯性残差和观测误差均作为扩展卡尔曼滤波模型的输入，其中，惯性残差用
Figure BDA0002843728310000101
表示，观测误差用Δrn表示，实际坐标用rn表示，观测坐标用
Figure BDA0002843728310000102
表示，预设路径坐标用
Figure BDA0002843728310000103
表示。
步骤S222、将所述惯性残差和所述观测误差作为扩展卡尔曼滤波模型的输入，输出对所述当前第一位置和所述当前第一姿态的误差修正量。
步骤S223、根据所述误差修正量纠正所述当前第一位置和所述当前第一姿态，分别得到所述当前第二位置和所述当前第二姿态。
具体地，将行进过程中每个位置点对应的惯性残差和观测误差不断输入扩展卡尔曼滤波模型中，可以得到每个位置点的误差修正量，通过根据误差修正量对当前第一位置和当前第一姿态进行修正，即可得到每个位置点修正后的坐标和姿态，即当前第二位置和当前第二姿态。
在一具体实施例中，在步骤S223之后包括：
步骤S224、根据所有的当前第二位置和所有的当前第二姿态确定纠正后的第一行进轨迹。
步骤S225、通过RIS平滑算法对所述第一行进轨迹进行反向平滑处理，之后进行内插，得到第二行进轨迹。
具体地，所有的当前第二位置和所有的当前第二姿态组成了第一行进轨迹，通过RTS平滑算法对第一行进国际进行反向平滑处理及内插，即可对机器人的行进轨迹进行优化。
当通过激光器获取隧道内控制点的当前第一激光点云时，若控制点的数量多则当前第一激光点云较多，反之越少，因此，当控制点较多时机器人可以不间断行走，获得完整的隧道三维点云；当控制点较少时，机器人需要间断行走，在多段隧道中获取当前第一激光点云，并对多段当前第一激光点云进行拼接，之后对拼接的当前第一激光点云进行处理即可得到完整的隧道三维激光点云。
在一具体实施例中，如图8所示，步骤S400包括：
步骤S401、对每个对应的所述当前第一激光点云、所述当前第二位置和所述当前第二姿态分别进行坐标系转换，在测图坐标系中融合解算得到当前第二激光点云。
具体地，在机器人行进过程中通过激光器测量机器人至隧道的距离信息，结合当前第一位置和当前第一姿态进行计算可得到当前第一激光点云。在行进过程中，通过控制点可不断修正机器人的当前第一位置和当前第一姿态，进而得到修正后的当前第二位置和当前第二姿态，通过对对当前第二位置和当前第二姿态与当前第一激光点云进行融合解算，即可得到当前第二激光点云。
步骤S402、根据所述第二行进轨迹和所有当前第二激光点云，得到隧道三维点云。
具体地，而根据第二行进轨迹和当前第二激光点云的分布，即可得到隧道整体的三维点云，且三维点云具有高精度。
进一步地，对坐标进行融合解算的步骤如下：
当前第一激光点云在激光器坐标系(s系)下的坐标表示为：
ns＝[cosαsinβ，cosαsinβ,sinα]T，xs＝p·ns
其中，α是激光器发射水平角，β是激光器发射竖直角，p为激光测距值，ns为激光射线的方向向量。
激光器坐标系(s系)转换到载体坐标系(b系)，转换公式如下：
Figure BDA0002843728310000111
其中，
Figure BDA0002843728310000112
为s系原点到b系原点的三维平移分量，也称杆臂值，
Figure BDA0002843728310000113
是s系相对b系的选择矩阵；
当通过双目立体相机采集到控制点后，则将载体坐标系的观测值解算到测图坐标系(即导航坐标系)中，其转换公式如下；
Figure BDA0002843728310000114
其中，
Figure BDA0002843728310000115
为机器人当前第二位置的坐标信息，
Figure BDA0002843728310000116
为b系到n系的旋转矩阵，旋转矩阵按照当前第二位置坐标向量中的三个欧拉角θx,θy,θzz-x-y旋转顺序左乘得到，其转换公式为：
Figure BDA0002843728310000121
通过将激光器获取的当前第一激光点云与当前第二位置和当前第二姿态通过坐标系转换，将激光器获取的当前第一激光点云转换到载体坐标系再到导航坐标系中，可以推算出激光器获取的隧道云数据的精确坐标。
对于长距离隧道环境，惯性系统长时间误差累积会加大，虽然可以通过控制点进行改善，对于控制点稀疏或者前期无控制点隧道，则需要进行分段采集计算，因此，本发明针对控制点稀疏的隧道则采取分段计算的方式，因此，在另一具体实施例中，如图9所示，步骤S400还包括：
步骤S410、当存在多段所述第二行进轨迹时，分别对每段所述第二行进轨迹中的所述当前第二位置、所述当前第二姿态和所述当前第一激光点云进行坐标系转换，在测图坐标系中解算得到多段所述第二行进轨迹对应的所述当前第二激光点云。
具体地，采取分段计算时，机器人系统启动时先静止一段时间，以便于机器人的惯性导航单元的状态收敛，从而能够比较准确的测量出其系统的零偏误差。然后控制激光器开始扫描，随后停止扫描，机器人行驶一段距离后再次停下，经过一段时间静止后扫描仪再继续扫描，以此往复，类似于“停(扫描)-走-停(扫描)”的定点扫描模式。通过此种定点扫描模式进行扫描，能够获取每段路程对应的多段当前第一激光点云。
步骤S420、将多段所述第二行进轨迹中的所述当前第二激光点云根据所述第二行进轨迹分别进行初步拼接，得到隧道的多段初始激光点云。
具体地，通过对各段第二行进轨迹中的各当前第二激光点云进行拼接，可以得到多段初始激光点云。
进一步地，通过载体坐标系即可完成对激光器坐标的转换，进而将各个扫描点的坐标统一到载体坐标系中，实现测站点间三维点云数据的初步拼接。而进行坐标系转换可参照上述激光器坐标系转换到载体坐标系再到导航坐标系的方式，此处不再赘述。
在一具体实施例中，如图9所示，在步骤S420之后还包括：
步骤S430、提取多段所述初始激光点云中重合的点云进行配准。
对经处理的多段激光点云进行拼接，并对相邻激光点云中重合部分进行配准，以实现精确配置，保证拼接过程中的精准度。
步骤S440、对配准后的多段所述初始激光点云进行拼接，得到完整的隧道三维点云。
通过对精准的三维点云进行拼接即可得到完整的隧道三维点云，实现在控制点稀疏情况下对隧道的高精度测量。
本发明通过机器人启动后利用标定好的双目立体相机确定机器人初始位置和初始姿态，之后通过惯性导航单元获取机器人自身的当前第一位置和当前第一姿态，并结合里程计获取的位置增量对当前第一位置和当前第一姿态计算惯性残差，并结合观测点与当前第一位置和当前第一姿态之间观测误差，进而更新位置和姿态得到当前第二位置和当前第二姿态。之后借助预设路线与当前第二位置实现机器人在隧道中自主导航行走，且行走过程中不断获取当前第一激光点云，进而根据当前第二位置和当前第二姿态对当前第一激光点云进行解算，得到精确的当前第二激光点云，进而得到隧道的三维点云。
进一步地，如图10所示，基于上述基于机器人平台的隧道激光点云采集方法，本发明还相应提供了一种机器人系统，所述机器人系统包括处理器10、存储器20及显示器30。图10仅示出了机器人系统的部分组件，但是应理解的是，并不要求实施所有示出的组件，可以替代的实施更多或者更少的组件。
所述存储器20在一些实施例中可以是所述机器人系统的内部存储单元，例如机器人系统的硬盘或内存。所述存储器20在另一些实施例中也可以是所述机器人系统的外部存储设备，例如所述机器人系统上配备的插接式硬盘，智能存储卡(Smart Media Card,SMC)，安全数字(Secure Digital,SD)卡，闪存卡(Flash Card)等。进一步地，所述存储器20还可以既包括所述机器人系统的内部存储单元也包括外部存储设备。所述存储器20用于存储安装于所述机器人系统的应用软件及各类数据，例如所述安装机器人系统的程序代码等。所述存储器20还可以用于暂时地存储已经输出或者将要输出的数据。在一实施例中，存储器20上存储有基于机器人系统的隧道激光点云采集程序40，该基于机器人系统的隧道激光点云采集程序40可被处理器10所执行，从而实现本申请中基于机器人平台的隧道激光点云采集方法。
所述处理器10在一些实施例中可以是一中央处理器(Central Processing Unit,CPU)，微处理器或其他数据处理芯片，用于运行所述存储器20中存储的程序代码或处理数据，例如执行所述基于机器人平台的隧道激光点云采集方法等。
所述显示器30在一些实施例中可以是LED显示器、液晶显示器、触控式液晶显示器以及OLED(Organic Light-Emitting Diode，有机发光二极管)触摸器等。所述显示器30用于显示在所述机器人系统的信息以及用于显示可视化的用于界面。所述机器人系统的部件10-30通过系统总线相互通信。
而所述机器人系统还包括激光器、双目立体相机、里程计和惯性导航单元，其中，所述激光器用于激光测距，所述双目立体相机用于拍摄控制点图像，所述里程计用于测量机器人行走过程中的位置增量，所述惯性导航单元用于测量当前位置和当前姿态。
而所述激光器、双目立体相机、里程计和惯性导航单元均被所述基于机器人系统的隧道激光点云采集程序40控制。在一实施例中，当处理器10执行所述存储器20中基于机器人系统的隧道激光点云采集程序40时实现以下步骤：
获取当前第一位置、当前第一姿态及隧道内预设控制点，确定当前第一激光点云；
根据所述预设控制点纠正所述当前第一位置和所述当前第一姿态，得到当前第二位置和当前第二姿态；
建立所述当前第二位置、所述当前第二姿态与预设路线之间的匹配关系，确定导航路线；
根据所述当前第二位置、所述当前第二姿态和所述导航路线，确定隧道三维点云。
所述建立所述当前第二位置、所述当前第二姿态与预设路线之间的匹配关系，确定导航路线包括：
通过地图匹配算法，建立所述当前第二位置与所述预设路线的匹配关系；
根据所述匹配关系确定所述当前第二位置和所述当前第二姿态与所述预设路线之间的偏差参数；
按照所述偏差参数调整行进过程中的行进参数，得到所述导航路线。
所述根据所述预设控制点纠正所述当前第一位置和所述当前第一姿态，得到当前第二位置和当前第二姿态包括：
对比所述预设控制点的观测坐标与多个预先实测得到的控制点的实际坐标，确定观测所述预设控制点的观测误差；
根据所述观测误差对所述当前第一位置和所述当前一姿态进行纠正，得到所述当前第二位置和所述当前第二姿态。
所述根据所述观测误差对所述当前第一位置和所述当前一姿态进行纠正，得到所述当前第二位置和所述当前第二姿态包括：
获取行进过程中的位置增量，根据所述位置增量确定所述当前第一位置和所述当前第一姿态的惯性残差；
将所述惯性残差和所述观测误差作为扩展卡尔曼滤波模型的输入，输出对所述当前第一位置和所述当前第一姿态的误差修正量；
根据所述误差修正量纠正所述当前第一位置和所述当前第一姿态，分别得到所述当前第二位置和所述当前第二姿态。
所述根据所述误差修正量纠正所述当前第一位置和所述当前第一姿态，分别得到所述当前第二位置和所述当前第二姿态，之后还包括：
根据所有的当前第二位置和所有的当前第二姿态确定纠正后的第一行进轨迹；
通过RIS平滑算法对所述第一行进轨迹进行反向平滑处理，之后进行内插，得到第二行进轨迹。
所述根据所述当前第二位置、所述当前第二姿态和所述导航路线，确定隧道三维点云包括：
对每个对应的所述当前第一激光点云、所述当前第二位置和所述当前第二姿态分别进行坐标系转换，在测图坐标系中融合解算得到当前第二激光点云；
根据所述第二行进轨迹和所有当前第二激光点云，得到隧道三维点云。
所述根据所述当前第二位置、所述当前第二姿态和所述导航路线，确定隧道三维点云还包括：
当存在多段所述第二行进轨迹时，分别对每段所述第二行进轨迹中的所述当前第二位置、所述当前第二姿态和所述当前第一激光点云进行坐标系转换，在测图坐标系中解算得到多段所述第二行进轨迹对应的所述当前第二激光点云；
将多段所述第二行进轨迹中的所述当前第二激光点云根据所述第二行进轨迹分别进行初步拼接，得到隧道的多段初始激光点云。
所述将多段所述第二行进轨迹中的所述当前第二激光点云根据所述第二行进轨迹分别进行初步拼接，得到隧道的多段初始激光点云，之后还包括：
提取多段所述初始激光点云中重合的点云进行配准；
对配准后的多段所述初始激光点云进行拼接，得到完整的隧道三维点云。
所述获取当前第一位置、当前第一姿态及隧道内预设控制点，确定当前第一激光点云，之前还包括：
通过双目立体相机获取隧道内预设控制点的图像；
根据与所述双目立体相机的标定关系以及所拍摄图像的立体像对的后方交会，确定初始位置和初始姿态。
本发明还提供一种存储介质，其中，所述存储介质存储有基于机器人平台的隧道激光点云采集程序，所述基于机器人平台的隧道激光点云采集程序被处理器执行时实现如上所述的基于机器人平台的隧道激光点云采集方法的步骤。
综上所述，本发明公开的基于机器人平台的隧道激光点云采集方法及机器人系统，本发明通过机器人启动后利用标定好的双目立体相机确定机器人初始位置和初始姿态，之后通过惯性导航单元获取机器人自身的当前第一位置和当前第一姿态，并结合里程计获取的位置增量对当前第一位置和当前第一姿态计算惯性残差，并结合观测点与当前第一位置和当前第一姿态之间观测误差，进而更新位置和姿态得到当前第二位置和当前第二姿态。之后借助预设路线与当前第二位置实现机器人在隧道中自主导航行走，且行走过程中不断获取当前第一激光点云，进而根据当前第二位置和当前第二姿态对当前第一激光点云进行解算，得到精确的当前第二激光点云，进而得到隧道的三维点云。
当然，本领域普通技术人员可以理解实现上述实施例方法中的全部或部分流程，是可以通过计算机程序来指令相关硬件(如处理器，控制器等)来完成，所述的程序可存储于一计算机可读取的存储介质中，所述程序在执行时可包括如上述各方法实施例的流程。其中所述的存储介质可为存储器、磁碟、光盘等。
应当理解的是，本发明的应用不限于上述的举例，对本领域普通技术人员来说，可以根据上述说明加以改进或变换，所有这些改进和变换都应属于本发明所附权利要求的保护范围。

Claims (4)

1.一种基于机器人平台的隧道激光点云采集方法，其特征在于，包括：
获取当前第一位置、当前第一姿态及隧道内预设控制点，确定当前第一激光点云；
对比所述预设控制点的观测坐标与多个预先实测得到的控制点的实际坐标，确定观测所述预设控制点的观测误差；其中，机器人上配置有标定好的双目立体相机，通过双目立体相机拍摄隧道中预先设置的控制点图像，通过对图像进行分析，能够得到预设控制点的观测坐标；预设控制点的实际坐标是预先测量好的标准坐标；
根据所述观测误差对所述当前第一位置和所述当前第一姿态进行纠正，得到当前第二位置和当前第二姿态；其中包括：获取行进过程中的位置增量，根据所述位置增量确定所述当前第一位置和所述当前第一姿态的惯性残差；将所述惯性残差和所述观测误差作为扩展卡尔曼滤波模型的输入，输出对所述当前第一位置和所述当前第一姿态的误差修正量；根据所述误差修正量纠正所述当前第一位置和所述当前第一姿态，分别得到所述当前第二位置和所述当前第二姿态；根据所有的当前第二位置和所有的当前第二姿态确定纠正后的第一行进轨迹；通过RIS平滑算法对所述第一行进轨迹进行反向平滑处理，之后进行内插，得到第二行进轨迹；
通过地图匹配算法，建立所述当前第二位置与预设路线的匹配关系；
根据所述匹配关系确定所述当前第二位置和所述当前第二姿态与所述预设路线之间的偏差参数；其中，根据当前第二位置和当前第二姿态确定当前坐标点，将该坐标点与预设路线上对应位置点的坐标进行比对，确定当前行进路径的角度偏差，通过机器人系统坐标系和预设路线坐标系之间的关系调整机器人的行进方向；
按照所述偏差参数调整行进过程中的行进参数，得到导航路线，其中，通过偏差参数对机器人的位置、姿态和速度参数进行修正，确定机器人的所述导航路线，为机器人在隧道中行走提供依据；
根据所述当前第二位置、所述当前第二姿态和所述导航路线，确定隧道三维点云；其中包括：
对每个对应的所述当前第一激光点云、所述当前第二位置和所述当前第二姿态分别进行坐标系转换，在测图坐标系中融合解算得到当前第二激光点云；
在测图坐标系中融合解算的步骤如下：
当前第一激光点云在激光器坐标系即s系下的坐标表示为：
ns＝[cosαsinβ，cosαsinβ,sinα]T，xs＝p·ns
其中，xs是当前第一激光点云在s系下的坐标，α是激光器发射水平角，β是激光器发射竖直角，p为激光测距值，ns为激光射线的方向向量；
激光器坐标系即s系转换到载体坐标系即b系，转换公式如下：
Figure FDA0003499161240000021
其中，xb是当前第一激光点云从s系转换到b系下的坐标，
Figure FDA0003499161240000022
为s系原点到b系原点的三维平移分量，也称杆臂值，
Figure FDA0003499161240000023
是s系相对b系的选择矩阵；
当通过双目立体相机采集到控制点后，则将载体坐标系的观测值解算到测图坐标系即导航坐标系中，其转换公式如下：
Figure FDA0003499161240000024
其中，xn是当前第一激光点云从b系转换到导航坐标系下的坐标，
Figure FDA0003499161240000025
为机器人当前第二位置的坐标信息，
Figure FDA0003499161240000026
为b系到n系的旋转矩阵，旋转矩阵按照当前第二位置坐标向量中的三个欧拉角θx,θy,θz z-x-y旋转顺序左乘得到，其转换公式为：
Figure FDA0003499161240000027
Figure FDA0003499161240000031
通过将激光器获取的当前第一激光点云与当前第二位置和当前第二姿态通过坐标系转换，将激光器获取的当前第一激光点云转换到载体坐标系再到导航坐标系中，推算出激光器获取的隧道云数据的精确坐标；
其中对于控制点稀疏或者前期无控制点隧道，进行分段采集计算，其中，机器人系统启动时先静止一段时间，惯性导航单元的状态收敛，从而测量出机器人系统的零偏误差，然后控制激光器开始扫描，随后停止扫描，机器人行驶一段距离后再次停下，经过一段时间静止后扫描仪再继续扫描，以此往复，获取每段路程对应的多段当前第一激光点云；
当分段采集计算时，所述根据所述当前第二位置、所述当前第二姿态和导航路线，确定隧道三维点云还包括：
当存在多段所述第二行进轨迹时，分别对每段所述第二行进轨迹中的所述当前第二位置、所述当前第二姿态和所述当前第一激光点云进行坐标系转换，在测图坐标系中解算得到多段所述第二行进轨迹对应的所述当前第二激光点云；其中，将各个扫描点的坐标统一到载体坐标系中；
将多段所述第二行进轨迹中的所述当前第二激光点云根据所述第二行进轨迹分别进行初步拼接，得到隧道的多段初始激光点云；
提取多段所述初始激光点云中重合的点云进行配准；
对配准后的多段所述初始激光点云进行拼接，得到完整的隧道三维点云。
2.根据权利要求1所述的基于机器人平台的隧道激光点云采集方法，其特征在于，所述根据所述当前第二位置、所述当前第二姿态和所述导航路线，确定隧道三维点云还包括：
根据所述第二行进轨迹和所有当前第二激光点云，得到隧道三维点云。
3.根据权利要求1所述的基于机器人平台的隧道激光点云采集方法，其特征在于，所述获取当前第一位置、当前第一姿态及隧道内预设控制点，确定当前第一激光点云，之前还包括：
通过双目立体相机获取隧道内预设控制点的图像；
根据与所述双目立体相机的标定关系以及所拍摄图像的立体像对的后方交会，确定初始位置和初始姿态。
4.一种机器人系统，其特征在于，所述机器人系统包括：存储器、处理器及存储在所述存储器上并可在所述处理器上运行的基于机器人系统的隧道激光点云采集程序，所述基于机器人系统的隧道激光点云采集程序被所述处理器执行时实现如权利要求1-3任一项所述的基于机器人平台的隧道激光点云采集方法的步骤。
CN202011501913.3A 2020-12-17 2020-12-17 基于机器人平台的隧道激光点云采集方法及机器人系统 Active CN112729252B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202011501913.3A CN112729252B (zh) 2020-12-17 2020-12-17 基于机器人平台的隧道激光点云采集方法及机器人系统

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202011501913.3A CN112729252B (zh) 2020-12-17 2020-12-17 基于机器人平台的隧道激光点云采集方法及机器人系统

Publications (2)

Publication Number Publication Date
CN112729252A CN112729252A (zh) 2021-04-30
CN112729252B true CN112729252B (zh) 2022-05-20

Family

ID=75602974

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202011501913.3A Active CN112729252B (zh) 2020-12-17 2020-12-17 基于机器人平台的隧道激光点云采集方法及机器人系统

Country Status (1)

Country Link
CN (1) CN112729252B (zh)

Families Citing this family (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115560735B (zh) * 2022-08-26 2024-10-29 中国长江三峡集团有限公司 一种用于隧洞设备移动定位的复合标靶系统及定位方法
CN116934751B (zh) * 2023-09-15 2024-01-12 深圳市信润富联数字科技有限公司 高精点云的采集方法及装置、存储介质、电子设备

Family Cites Families (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111007530B (zh) * 2019-12-16 2022-08-12 武汉汉宁轨道交通技术有限公司 激光点云数据处理方法、装置及系统

Also Published As

Publication number Publication date
CN112729252A (zh) 2021-04-30

Similar Documents

Publication Publication Date Title
CN110160542B (zh) 车道线的定位方法和装置、存储介质、电子装置
CN108931795B (zh) 定位设备轨迹优化和边界提取方法及装置
CN111707272B (zh) 一种地下车库自动驾驶激光定位系统
CN113654555A (zh) 一种基于多传感器数据融合的自动驾驶车辆高精定位方法
CN112183133A (zh) 一种基于ArUco码引导的移动机器人自主充电方法
CN114252082B (zh) 车辆定位方法、装置和电子设备
CN112729252B (zh) 基于机器人平台的隧道激光点云采集方法及机器人系统
CN109579838A (zh) Agv小车的定位方法及定位系统
US12118666B2 (en) Method, device and system for cooperatively constructing point cloud map
CN110764110B (zh) 路径导航方法、装置及计算机可读存储介质
CN111857114A (zh) 一种机器人编队移动方法、系统、设备和存储介质
CN108759822B (zh) 一种移动机器人3d定位系统
CN116380039A (zh) 一种基于固态激光雷达和点云地图的移动机器人导航系统
CN114115263A (zh) 用于agv的自主建图方法、装置、移动机器人及介质
CN114964213B (zh) 基于姿态感知和视觉扫描的建筑工程施工定位系统及方法
CN112508933B (zh) 一种基于复杂空间障碍物定位的柔性机械臂运动避障方法
CN116429121A (zh) 基于多传感器的定位方法、装置、自移动设备及存储介质
Nowicki A data-driven and application-aware approach to sensory system calibration in an autonomous vehicle
CN113821023B (zh) 一种用于智能地坪划线机器人的划线方法
CN215494708U (zh) 一种智能地坪划线机器人
CN116929336A (zh) 一种基于最小误差的激光反光柱slam建图方法
CN116643289A (zh) 一种附合导线约束的井下巷道激光雷达slam方法
CN115265534A (zh) 基于AprilTag码的多传感器融合定位导航方法、装置及系统
US20230021828A1 (en) Maintenance support system
CN116858276B (zh) 一种地图辅助的多传感器融合定位方法及计算机可读介质

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
GR01 Patent grant
GR01 Patent grant