CN112344937A - 一种基于单棱镜旋转装置的掘进机位姿测量方法及系统 - Google Patents

一种基于单棱镜旋转装置的掘进机位姿测量方法及系统 Download PDF

Info

Publication number
CN112344937A
CN112344937A CN202011164836.7A CN202011164836A CN112344937A CN 112344937 A CN112344937 A CN 112344937A CN 202011164836 A CN202011164836 A CN 202011164836A CN 112344937 A CN112344937 A CN 112344937A
Authority
CN
China
Prior art keywords
heading machine
determining
vector
angle
total station
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.)
Granted
Application number
CN202011164836.7A
Other languages
English (en)
Other versions
CN112344937B (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.)
Xian University of Science and Technology
Original Assignee
Xian University of Science and Technology
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 Xian University of Science and Technology filed Critical Xian University of Science and Technology
Priority to CN202011164836.7A priority Critical patent/CN112344937B/zh
Publication of CN112344937A publication Critical patent/CN112344937A/zh
Application granted granted Critical
Publication of CN112344937B publication Critical patent/CN112344937B/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
    • 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)
  • Automation & Control Theory (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Length Measuring Devices By Optical Means (AREA)
  • Length Measuring Devices With Unspecified Measuring Means (AREA)
  • Testing Of Balance (AREA)

Abstract

本发明公开了一种基于单棱镜旋转装置的掘进机位姿测量方法及系统。该方法包括：获取全站仪坐标系下第一棱镜在第一位置、第二位置和第三位置的坐标；确定第一位置至中点位置的向量为第一向量，确定第二位置至第三位置的向量为第二向量；根据第一向量和全站仪坐标系的YOZ平面的法向量确定掘进机的偏航角；根据第一向量和全站仪坐标系的XOY平面的法向量确定掘进机的俯仰角；根据第二向量和全站仪坐标系的XOY平面的法向量确定掘进机的横滚角；根据第一位置的坐标、偏航角、俯仰角和横滚角确定掘进机位姿。采用本发明的方法及系统，利用一个棱镜测量得到掘进机位姿，测量简单，实用性强。

Description

一种基于单棱镜旋转装置的掘进机位姿测量方法及系统
技术领域
本发明涉及智能掘进机器人技术领域，特别是涉及一种基于单棱镜旋转装置的掘进机位姿测量方法及系统。
背景技术
在自动化和信息化的大背景下，对悬臂式掘进机的智能化改造受到了行业的重视，其中掘进机的自主定位成为了巷道智能化掘进的关键。目前，主要有惯导、视觉、IGPS、激光靶和全站仪等方法用于掘进机定位，其中全站仪测量位姿方法相对成熟，并在盾构施工中得到广泛应用，因此，一些学者借鉴盾构机位姿测量，提出了基于全站仪的掘进机位姿测量方法。然而，关于掘进机位姿测量的方法需要在掘进机机身上安装多个棱镜，并且全站仪测量方法只能得到掘进机位置信息，需结合其他测量手段才能测量出掘进机姿态信息。此外，传统掘进机测量方法需要多种设备，过于复杂，实用性差，限制了井下应用。
发明内容
本发明的目的是提供一种基于单棱镜旋转装置的掘进机位姿测量方法及系统，利用一个棱镜测量得到掘进机位姿，测量简单，实用性强。
为实现上述目的，本发明提供了如下方案：
一种基于单棱镜旋转装置的掘进机位姿测量方法，所述掘进机位姿测量方法应用掘进机位姿测量装置；所述掘进机位姿测量装置包括：设置在掘进机上的基座、设置在所述基座上的第一棱镜、位于所述掘进机和第二棱镜之间的全站仪；所述第二棱镜用于建立全站仪坐标系；
所述掘进机位姿测量方法，包括：
获取所述全站仪坐标系下所述第一棱镜在第一位置、第二位置和第三位置的坐标；
确定所述第一位置至中点位置的向量为第一向量，确定所述第二位置至所述第三位置的向量为第二向量；所述中点位置为所述第二位置和所述第三位置的中点位置；
根据所述第一向量和所述全站仪坐标系的YOZ平面的法向量确定掘进机的偏航角；
根据所述第一向量和所述全站仪坐标系的XOY平面的法向量确定掘进机的俯仰角；
根据所述第二向量和所述全站仪坐标系的XOY平面的法向量确定掘进机的横滚角；
根据所述第一位置的坐标、所述偏航角、所述俯仰角和所述横滚角确定掘进机位姿；所述掘进机位姿包括掘进机的姿态和位置，将所述第一位置的坐标确定为所述掘进机的位置，将所述偏航角、所述俯仰角和所述横滚角确定为所述掘进机的姿态。
可选的，所述第一位置位于所述掘进机的轴线上，所述第二位置和所述第三位置形成的直线与所述掘进机的轴线垂直，并且所述第一位置、所述第二位置和所述第三位置形成的三角形为等边三角形。
可选的，所述确定所述第一位置至中点位置的向量为第一向量，确定所述第二位置至所述第三位置的向量为第二向量，具体包括：
根据如下公式确定所述中点位置：
Figure BDA0002745415650000021
根据如下公式确定所述第一向量
Figure BDA0002745415650000022
Figure BDA0002745415650000023
根据如下公式确定所述第二向量
Figure BDA0002745415650000024
Figure BDA0002745415650000025
式中，第一位置的坐标为(xA,yA,zA)，第二位置的坐标为(xB,yB,zB)，第三位置的坐标为(xC,yC,zC)，中点位置坐标为(xD,yD,zD)。
可选的，所述根据所述第一向量和所述全站仪坐标系的YOZ平面的法向量确定掘进机的偏航角，具体包括：
根据
Figure BDA0002745415650000031
确定偏航角的初始值，在
Figure BDA0002745415650000032
时确定掘进机的偏航角为
Figure BDA0002745415650000033
否则确定掘进机的偏航角为
Figure BDA0002745415650000034
其中，α'为偏航角的初始值，α为掘进机的偏航角，
Figure BDA0002745415650000035
为全站仪坐标系的YOZ平面的法向量。
可选的，所述根据所述第一向量和所述全站仪坐标系的XOY平面的法向量确定掘进机的俯仰角，具体包括：
根据
Figure BDA0002745415650000036
确定俯仰角的初始值，在
Figure BDA0002745415650000037
时确定掘进机的俯仰角为
Figure BDA0002745415650000038
否则确定掘进机的俯仰角为
Figure BDA0002745415650000039
其中，β'为俯仰角的初始值，β为掘进机的俯仰角，
Figure BDA00027454156500000310
为全站仪坐标系的XOY平面的法向量。
可选的，所述根据所述第二向量和所述全站仪坐标系的XOY平面的法向量确定掘进机的横滚角，具体包括：
根据
Figure BDA00027454156500000311
确定横滚角的初始值，在
Figure BDA00027454156500000312
时确定掘进机的横滚角为
Figure BDA00027454156500000313
否则确定掘进机的横滚角为
Figure BDA00027454156500000314
其中，γ'为横滚角的初始值，γ为掘进机的横滚角。
本发明还提供一种基于单棱镜旋转装置的掘进机位姿测量系统，所述掘进机位姿测量系统基于掘进机位姿测量装置；所述掘进机位姿测量装置包括：设置在掘进机上的基座、设置在所述基座上的第一棱镜、位于所述掘进机和第二棱镜之间的全站仪；所述第二棱镜用于建立全站仪坐标系；
所述掘进机位姿测量系统，包括：
位置获取模块，用于获取所述全站仪坐标系下所述第一棱镜在第一位置、第二位置和第三位置的坐标；
向量确定模块，用于确定所述第一位置至中点位置的向量为第一向量，确定所述第二位置至所述第三位置的向量为第二向量；所述中点位置为所述第二位置和所述第三位置的中点位置；
偏航角确定模块，用于根据所述第一向量和所述全站仪坐标系的YOZ平面的法向量确定掘进机的偏航角；
俯仰角确定模块，用于根据所述第一向量和所述全站仪坐标系的XOY平面的法向量确定掘进机的俯仰角；
横滚角确定模块，用于根据所述第二向量和所述全站仪坐标系的XOY平面的法向量确定掘进机的横滚角；
掘进机位姿确定模块，用于根据所述第一位置的坐标、所述偏航角、所述俯仰角和所述横滚角确定掘进机位姿；所述掘进机位姿包括掘进机的姿态和位置，将所述第一位置的坐标确定为所述掘进机的位置，将所述偏航角、所述俯仰角和所述横滚角确定为所述掘进机的姿态。
可选的，所述向量确定模块，具体包括：
中点位置确定单元，用于根据如下公式确定所述中点位置：
Figure BDA0002745415650000041
第一向量确定单元，用于根据如下公式确定所述第一向量
Figure BDA0002745415650000042
Figure BDA0002745415650000043
第二向量确定单元，用于根据如下公式确定所述第二向量
Figure BDA0002745415650000044
Figure BDA0002745415650000045
式中，第一位置的坐标为(xA,yA,zA)，第二位置的坐标为(xB,yB,zB)，第三位置的坐标为(xC,yC,zC)，中点位置坐标为(xD,yD,zD)。
可选的，所述第一位置位于所述掘进机的轴线上，所述第二位置和所述第三位置形成的直线与所述掘进机的轴线垂直，并且所述第一位置、所述第二位置和所述第三位置形成的三角形为等边三角形。
可选的，
所述偏航角确定模块，具体包括：
偏航角确定单元，用于根据
Figure BDA0002745415650000046
确定偏航角的初始值，在
Figure BDA0002745415650000047
时确定掘进机的偏航角为
Figure BDA0002745415650000048
否则确定掘进机的偏航角为
Figure BDA0002745415650000049
其中，α'为偏航角的初始值，α为掘进机的偏航角，
Figure BDA00027454156500000410
为全站仪坐标系的YOZ平面的法向量；
所述俯仰角确定模块，具体包括：
俯仰角确定单元，用于根据
Figure BDA00027454156500000411
确定俯仰角的初始值，在
Figure BDA00027454156500000412
时确定掘进机的俯仰角为
Figure BDA0002745415650000051
否则确定掘进机的俯仰角为
Figure BDA0002745415650000052
其中，β'为俯仰角的初始值，β为掘进机的俯仰角，
Figure BDA0002745415650000053
为全站仪坐标系的XOY平面的法向量；
所述横滚角确定模块，具体包括：
横滚角确定单元，用于根据
Figure BDA0002745415650000054
确定横滚角的初始值，在
Figure BDA0002745415650000055
时确定掘进机的横滚角为
Figure BDA0002745415650000056
否则确定掘进机的横滚角为
Figure BDA0002745415650000057
其中，γ'为横滚角的初始值，γ为掘进机的横滚角。
与现有技术相比，本发明的有益效果是：
本发明提出了一种基于单棱镜旋转装置的掘进机位姿测量方法及系统，获取全站仪坐标系下第一棱镜在第一位置、第二位置和第三位置的坐标；确定第一位置至中点位置的向量为第一向量，确定第二位置至第三位置的向量为第二向量；根据第一向量和全站仪坐标系的YOZ平面的法向量确定掘进机的偏航角；根据第一向量和全站仪坐标系的XOY平面的法向量确定掘进机的俯仰角；根据第二向量和全站仪坐标系的XOY平面的法向量确定掘进机的横滚角；根据第一位置的坐标、偏航角、俯仰角和横滚角确定掘进机位姿。本发明利用一个棱镜测量得到掘进机位姿，测量简单，实用性强。
附图说明
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。
图1为本发明实施例中掘进机位姿测量装置示意图；
图2为本发明实施例中掘进机位姿测量装置俯视图；
图3为本发明实施例中偏航角、俯仰角和横滚角示意图；
图4为本发明实施例中基于单棱镜旋转装置的掘进机位姿测量方法流程图；
图5为本发明实施例中ABC三个位置示意图。
具体实施方式
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
本发明的目的是提供一种基于单棱镜旋转装置的掘进机位姿测量方法及系统，利用一个棱镜测量得到掘进机位姿，测量简单，实用性强。
为使本发明的上述目的、特征和优点能够更加明显易懂，下面结合附图和具体实施方式对本发明作进一步详细的说明。
实施例
本发明的掘进机位姿测量方法应用掘进机位姿测量装置。图1为本发明实施例中掘进机位姿测量装置示意图，图2为本发明实施例中掘进机位姿测量装置俯视图，如图1-2所示，掘进机位姿测量装置包括：设置在掘进机1上的棱镜基座2、设置在棱镜基座2上的第一棱镜3、位于掘进机1和第二棱镜5之间的全站仪4；第二棱镜5的位置在已知后视点上，用来进行后视定向，即第二棱镜5用于建立全站仪坐标系；棱镜基座2带动第一棱镜3旋转；全站仪4用于测量第一棱镜三个测量位置(第一位置A、第二位置B和第三位置C)的坐标；第一位置A位于掘进机的轴线上，第二位置B和第三位置C形成的直线与掘进机的轴线垂直，并且第一位置A、第二位置B和第三位置C形成的三角形为等边三角形。第一棱镜3在基座2带动下持续转动，每到测量位置时停止5s，同时，全站仪4一直对第一棱镜3跟踪测量，并将第一棱镜3位置坐标信息传输给计算机。全站仪坐标系中，Y轴为巷道轴线方向且与掘进方向相同，X轴为水平面中Y轴逆时针旋转90度方向，Z轴根据X轴Y轴通过右手坐标系建立。其中，如图3所示，偏航角为掘进机纵轴轴线与平面YOZ的夹角；俯仰角为掘进机纵轴轴线与平面XOY的夹角；横滚角为掘进机横轴轴线与平面XOY的夹角。
图4为本发明实施例中基于单棱镜旋转装置的掘进机位姿测量方法流程图，如图4所示，一种基于单棱镜旋转装置的掘进机位姿测量方法，包括：
步骤101：获取全站仪坐标系下第一棱镜在第一位置、第二位置和第三位置的坐标。
步骤102：确定第一位置至中点位置的向量为第一向量，确定第二位置至第三位置的向量为第二向量；中点位置为第二位置和第三位置的中点位置。
如图5所示，圆形基座的圆心在掘进机中心上，位置A在掘进机轴线上。ABC三个位置在全站仪坐标系下的坐标。
步骤102，具体包括：
根据如下公式确定中点位置：
Figure BDA0002745415650000071
根据如下公式确定第一向量
Figure BDA0002745415650000072
Figure BDA0002745415650000073
根据如下公式确定第二向量
Figure BDA0002745415650000074
Figure BDA0002745415650000075
式中，第一位置的坐标为(xA,yA,zA)，第二位置的坐标为(xB,yB,zB)，第三位置的坐标为(xC,yC,zC)，中点位置坐标为(xD,yD,zD)。
步骤103：根据第一向量和全站仪坐标系的YOZ平面的法向量确定掘进机的偏航角。
步骤103，具体包括：
根据
Figure BDA0002745415650000076
确定偏航角的初始值，在
Figure BDA0002745415650000077
时确定掘进机的偏航角为
Figure BDA0002745415650000078
否则确定掘进机的偏航角为
Figure BDA0002745415650000079
其中，α'为偏航角的初始值，α为掘进机的偏航角，
Figure BDA00027454156500000710
为全站仪坐标系的YOZ平面的法向量。
步骤104：根据第一向量和全站仪坐标系的XOY平面的法向量确定掘进机的俯仰角。
步骤104，具体包括：
根据
Figure BDA00027454156500000711
确定俯仰角的初始值，在
Figure BDA00027454156500000712
时确定掘进机的俯仰角为
Figure BDA00027454156500000713
否则确定掘进机的俯仰角为
Figure BDA00027454156500000714
其中，β'为俯仰角的初始值，β为掘进机的俯仰角，
Figure BDA0002745415650000081
为全站仪坐标系的XOY平面的法向量。
步骤105：根据第二向量和全站仪坐标系的XOY平面的法向量确定掘进机的横滚角。
步骤105，具体包括：
根据
Figure BDA0002745415650000082
确定横滚角的初始值，在
Figure BDA0002745415650000083
时确定掘进机的横滚角为
Figure BDA0002745415650000084
否则确定掘进机的横滚角为
Figure BDA0002745415650000085
其中，γ'为横滚角的初始值，γ为掘进机的横滚角。
步骤106：根据第一位置的坐标、偏航角、俯仰角和横滚角确定掘进机位姿；掘进机位姿包括掘进机的姿态和位置，将第一位置的坐标确定为掘进机的位置，将偏航角、俯仰角和横滚角确定为掘进机的姿态。
本发明还提供一种基于单棱镜旋转装置的掘进机位姿测量系统，该系统基于掘进机位姿测量装置。掘进机位姿测量系统，包括：
位置获取模块，用于获取全站仪坐标系下第一棱镜在第一位置、第二位置和第三位置的坐标。
向量确定模块，用于确定第一位置至中点位置的向量为第一向量，确定第二位置至第三位置的向量为第二向量；中点位置为第二位置和第三位置的中点位置。
向量确定模块，具体包括：
中点位置确定单元，用于根据如下公式确定中点位置：
Figure BDA0002745415650000086
第一向量确定单元，用于根据如下公式确定第一向量
Figure BDA0002745415650000087
Figure BDA0002745415650000088
第二向量确定单元，用于根据如下公式确定第二向量
Figure BDA0002745415650000089
Figure BDA00027454156500000810
式中，第一位置的坐标为(xA,yA,zA)，第二位置的坐标为(xB,yB,zB)，第三位置的坐标为(xC,yC,zC)，中点位置坐标为(xD,yD,zD)。
偏航角确定模块，用于根据第一向量和全站仪坐标系的YOZ平面的法向量确定掘进机的偏航角。
偏航角确定模块，具体包括：
偏航角确定单元，用于根据
Figure BDA0002745415650000091
确定偏航角的初始值，在
Figure BDA0002745415650000092
时确定掘进机的偏航角为
Figure BDA0002745415650000093
否则确定掘进机的偏航角为
Figure BDA0002745415650000094
其中，α'为偏航角的初始值，α为掘进机的偏航角，
Figure BDA0002745415650000095
为全站仪坐标系的YOZ平面的法向量。
俯仰角确定模块，用于根据第一向量和全站仪坐标系的XOY平面的法向量确定掘进机的俯仰角。
俯仰角确定模块，具体包括：
俯仰角确定单元，用于根据
Figure BDA0002745415650000096
确定俯仰角的初始值，在
Figure BDA0002745415650000097
时确定掘进机的俯仰角为
Figure BDA0002745415650000098
否则确定掘进机的俯仰角为
Figure BDA0002745415650000099
其中，β'为俯仰角的初始值，β为掘进机的俯仰角，
Figure BDA00027454156500000910
为全站仪坐标系的XOY平面的法向量。
横滚角确定模块，用于根据第二向量和全站仪坐标系的XOY平面的法向量确定掘进机的横滚角。
横滚角确定模块，具体包括：
横滚角确定单元，用于根据
Figure BDA00027454156500000911
确定横滚角的初始值，在
Figure BDA00027454156500000912
时确定掘进机的横滚角为
Figure BDA00027454156500000913
否则确定掘进机的横滚角为
Figure BDA00027454156500000914
其中，γ'为横滚角的初始值，γ为掘进机的横滚角。
掘进机位姿确定模块，用于根据第一位置的坐标、偏航角、俯仰角和横滚角确定掘进机位姿；掘进机位姿包括掘进机的姿态和位置，将第一位置的坐标确定为掘进机的位置，将偏航角、俯仰角和横滚角确定为掘进机的姿态。
对于实施例公开的系统而言，由于其与实施例公开的方法相对应，所以描述的比较简单，相关之处参见方法部分说明即可。
本发明可通过基座带动测量棱镜旋转到三个测量位置，实现了只需要采用一个棱镜便能测量出掘进位姿，相比传统方法，本系统克服了传统方法需要多个棱镜测量，且只能测量位置，无法测量姿态的问题，本系统具有结构简单，测量方便等优点。在本掘进机位姿测量系统的基础上，本发明采用向量与平面夹角的方法来计算掘进机姿态，提供了区别于传统计算姿态的方法，配合一个棱镜来测量出掘进机的位姿。
本文中应用了具体个例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的方法及其核心思想；同时，对于本领域的一般技术人员，依据本发明的思想，在具体实施方式及应用范围上均会有改变之处。综上，本说明书内容不应理解为对本发明的限制。

Claims (10)

1.一种基于单棱镜旋转装置的掘进机位姿测量方法，其特征在于，所述掘进机位姿测量方法应用掘进机位姿测量装置；所述掘进机位姿测量装置包括：设置在掘进机上的基座、设置在所述基座上的第一棱镜、位于所述掘进机和第二棱镜之间的全站仪；所述第二棱镜用于建立全站仪坐标系；
所述掘进机位姿测量方法，包括：
获取所述全站仪坐标系下所述第一棱镜在第一位置、第二位置和第三位置的坐标；
确定所述第一位置至中点位置的向量为第一向量，确定所述第二位置至所述第三位置的向量为第二向量；所述中点位置为所述第二位置和所述第三位置的中点位置；
根据所述第一向量和所述全站仪坐标系的YOZ平面的法向量确定掘进机的偏航角；
根据所述第一向量和所述全站仪坐标系的XOY平面的法向量确定掘进机的俯仰角；
根据所述第二向量和所述全站仪坐标系的XOY平面的法向量确定掘进机的横滚角；
根据所述第一位置的坐标、所述偏航角、所述俯仰角和所述横滚角确定掘进机位姿；所述掘进机位姿包括掘进机的姿态和位置，将所述第一位置的坐标确定为所述掘进机的位置，将所述偏航角、所述俯仰角和所述横滚角确定为所述掘进机的姿态。
2.根据权利要求1所述的基于单棱镜旋转装置的掘进机位姿测量方法，其特征在于，所述第一位置位于所述掘进机的轴线上，所述第二位置和所述第三位置形成的直线与所述掘进机的轴线垂直，并且所述第一位置、所述第二位置和所述第三位置形成的三角形为等边三角形。
3.根据权利要求2所述的基于单棱镜旋转装置的掘进机位姿测量方法，其特征在于，所述确定所述第一位置至中点位置的向量为第一向量，确定所述第二位置至所述第三位置的向量为第二向量，具体包括：
根据如下公式确定所述中点位置：
Figure FDA0002745415640000021
根据如下公式确定所述第一向量
Figure FDA0002745415640000022
Figure FDA0002745415640000023
根据如下公式确定所述第二向量
Figure FDA0002745415640000024
Figure FDA0002745415640000025
式中，第一位置的坐标为(xA,yA,zA)，第二位置的坐标为(xB,yB,zB)，第三位置的坐标为(xC,yC,zC)，中点位置坐标为(xD,yD,zD)。
4.根据权利要求3所述的基于单棱镜旋转装置的掘进机位姿测量方法，其特征在于，所述根据所述第一向量和所述全站仪坐标系的YOZ平面的法向量确定掘进机的偏航角，具体包括：
根据
Figure FDA0002745415640000026
确定偏航角的初始值，在
Figure FDA0002745415640000027
时确定掘进机的偏航角为
Figure FDA0002745415640000028
否则确定掘进机的偏航角为
Figure FDA0002745415640000029
其中，α'为偏航角的初始值，α为掘进机的偏航角，
Figure FDA00027454156400000210
为全站仪坐标系的YOZ平面的法向量。
5.根据权利要求4所述的基于单棱镜旋转装置的掘进机位姿测量方法，其特征在于，所述根据所述第一向量和所述全站仪坐标系的XOY平面的法向量确定掘进机的俯仰角，具体包括：
根据
Figure FDA00027454156400000211
确定俯仰角的初始值，在
Figure FDA00027454156400000212
时确定掘进机的俯仰角为
Figure FDA00027454156400000213
否则确定掘进机的俯仰角为
Figure FDA00027454156400000214
其中，β'为俯仰角的初始值，β为掘进机的俯仰角，
Figure FDA00027454156400000215
为全站仪坐标系的XOY平面的法向量。
6.根据权利要求5所述的基于单棱镜旋转装置的掘进机位姿测量方法，其特征在于，所述根据所述第二向量和所述全站仪坐标系的XOY平面的法向量确定掘进机的横滚角，具体包括：
根据
Figure FDA00027454156400000216
确定横滚角的初始值，在
Figure FDA00027454156400000217
时确定掘进机的横滚角为
Figure FDA00027454156400000218
否则确定掘进机的横滚角为
Figure FDA00027454156400000219
其中，γ'为横滚角的初始值，γ为掘进机的横滚角。
7.一种基于单棱镜旋转装置的掘进机位姿测量系统，其特征在于，所述掘进机位姿测量系统基于掘进机位姿测量装置；所述掘进机位姿测量装置包括：设置在掘进机上的基座、设置在所述基座上的第一棱镜、位于所述掘进机和第二棱镜之间的全站仪；所述第二棱镜用于建立全站仪坐标系；
所述掘进机位姿测量系统，包括：
位置获取模块，用于获取所述全站仪坐标系下所述第一棱镜在第一位置、第二位置和第三位置的坐标；
向量确定模块，用于确定所述第一位置至中点位置的向量为第一向量，确定所述第二位置至所述第三位置的向量为第二向量；所述中点位置为所述第二位置和所述第三位置的中点位置；
偏航角确定模块，用于根据所述第一向量和所述全站仪坐标系的YOZ平面的法向量确定掘进机的偏航角；
俯仰角确定模块，用于根据所述第一向量和所述全站仪坐标系的XOY平面的法向量确定掘进机的俯仰角；
横滚角确定模块，用于根据所述第二向量和所述全站仪坐标系的XOY平面的法向量确定掘进机的横滚角；
掘进机位姿确定模块，用于根据所述第一位置的坐标、所述偏航角、所述俯仰角和所述横滚角确定掘进机位姿；所述掘进机位姿包括掘进机的姿态和位置，将所述第一位置的坐标确定为所述掘进机的位置，将所述偏航角、所述俯仰角和所述横滚角确定为所述掘进机的姿态。
8.根据权利要求7所述的基于单棱镜旋转装置的掘进机位姿测量系统，其特征在于，所述向量确定模块，具体包括：
中点位置确定单元，用于根据如下公式确定所述中点位置：
Figure FDA0002745415640000031
第一向量确定单元，用于根据如下公式确定所述第一向量
Figure FDA0002745415640000032
Figure FDA0002745415640000033
第二向量确定单元，用于根据如下公式确定所述第二向量
Figure FDA0002745415640000041
Figure FDA0002745415640000042
式中，第一位置的坐标为(xA,yA,zA)，第二位置的坐标为(xB,yB,zB)，第三位置的坐标为(xC,yC,zC)，中点位置坐标为(xD,yD,zD)。
9.根据权利要求8所述的基于单棱镜旋转装置的掘进机位姿测量系统，其特征在于，所述第一位置位于所述掘进机的轴线上，所述第二位置和所述第三位置形成的直线与所述掘进机的轴线垂直，并且所述第一位置、所述第二位置和所述第三位置形成的三角形为等边三角形。
10.根据权利要求9所述的基于单棱镜旋转装置的掘进机位姿测量系统，其特征在于，
所述偏航角确定模块，具体包括：
偏航角确定单元，用于根据
Figure FDA0002745415640000043
确定偏航角的初始值，在
Figure FDA0002745415640000044
时确定掘进机的偏航角为
Figure FDA0002745415640000045
否则确定掘进机的偏航角为
Figure FDA0002745415640000046
其中，α'为偏航角的初始值，α为掘进机的偏航角，
Figure FDA0002745415640000047
为全站仪坐标系的YOZ平面的法向量；
所述俯仰角确定模块，具体包括：
俯仰角确定单元，用于根据
Figure FDA0002745415640000048
确定俯仰角的初始值，在
Figure FDA0002745415640000049
时确定掘进机的俯仰角为
Figure FDA00027454156400000410
否则确定掘进机的俯仰角为
Figure FDA00027454156400000411
其中，β'为俯仰角的初始值，β为掘进机的俯仰角，
Figure FDA00027454156400000412
为全站仪坐标系的XOY平面的法向量；
所述横滚角确定模块，具体包括：
横滚角确定单元，用于根据
Figure FDA00027454156400000413
确定横滚角的初始值，在
Figure FDA00027454156400000414
时确定掘进机的横滚角为
Figure FDA00027454156400000415
否则确定掘进机的横滚角为
Figure FDA00027454156400000416
其中，γ'为横滚角的初始值，γ为掘进机的横滚角。
CN202011164836.7A 2020-10-27 2020-10-27 一种基于单棱镜旋转装置的掘进机位姿测量方法及系统 Active CN112344937B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202011164836.7A CN112344937B (zh) 2020-10-27 2020-10-27 一种基于单棱镜旋转装置的掘进机位姿测量方法及系统

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202011164836.7A CN112344937B (zh) 2020-10-27 2020-10-27 一种基于单棱镜旋转装置的掘进机位姿测量方法及系统

Publications (2)

Publication Number Publication Date
CN112344937A true CN112344937A (zh) 2021-02-09
CN112344937B CN112344937B (zh) 2022-08-16

Family

ID=74358658

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202011164836.7A Active CN112344937B (zh) 2020-10-27 2020-10-27 一种基于单棱镜旋转装置的掘进机位姿测量方法及系统

Country Status (1)

Country Link
CN (1) CN112344937B (zh)

Cited By (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113252063A (zh) * 2021-05-25 2021-08-13 中国煤炭科工集团太原研究院有限公司 一种基于全站仪的采掘装备进深测量方法
CN113252043A (zh) * 2021-05-25 2021-08-13 中国煤炭科工集团太原研究院有限公司 一种基于全站仪的掘进装备机身偏差测量方法
CN113340285A (zh) * 2021-06-18 2021-09-03 中铁工程装备集团有限公司 基于协作机械臂的主机械臂末端位姿检测方法及检测装置
CN115218862A (zh) * 2022-06-16 2022-10-21 上海隧道工程有限公司 基于全站仪的盾构施工管片沉降自动监测系统及监测方法
CN113048972B (zh) * 2021-03-12 2023-05-23 衢州海西电子科技有限公司 矿山工程机械姿态位置确定方法及系统

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2003082990A (ja) * 2001-09-13 2003-03-19 Nippon Koki Kk トンネル掘削機の位置計測装置
JP2010085311A (ja) * 2008-10-01 2010-04-15 Nishimatsu Constr Co Ltd インバート部の掘削管理方法
CN103322989A (zh) * 2013-06-06 2013-09-25 上海米度测量技术有限公司 一种动态实时测量顶管机位置和姿态的测量装置及其方法
CN108269286A (zh) * 2016-12-30 2018-07-10 中国空气动力研究与发展中心超高速空气动力研究所 基于组合立体标志的多相机位姿关联方法
CN109115173A (zh) * 2018-08-23 2019-01-01 西安科技大学 基于直线定位模型的掘进机机身位姿单目视觉测量方法
WO2020049497A1 (en) * 2018-09-07 2020-03-12 Saipem S.P.A. System and method for positioning an offshore structure

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2003082990A (ja) * 2001-09-13 2003-03-19 Nippon Koki Kk トンネル掘削機の位置計測装置
JP2010085311A (ja) * 2008-10-01 2010-04-15 Nishimatsu Constr Co Ltd インバート部の掘削管理方法
CN103322989A (zh) * 2013-06-06 2013-09-25 上海米度测量技术有限公司 一种动态实时测量顶管机位置和姿态的测量装置及其方法
CN108269286A (zh) * 2016-12-30 2018-07-10 中国空气动力研究与发展中心超高速空气动力研究所 基于组合立体标志的多相机位姿关联方法
CN109115173A (zh) * 2018-08-23 2019-01-01 西安科技大学 基于直线定位模型的掘进机机身位姿单目视觉测量方法
WO2020049497A1 (en) * 2018-09-07 2020-03-12 Saipem S.P.A. System and method for positioning an offshore structure

Non-Patent Citations (4)

* Cited by examiner, † Cited by third party
Title
DIMITRIS S.PARAFOROS 等: "Total station data assessment using an industrial robotic arm for dynamic 3D in-field positioning with sub-centimetre accuracy", 《COMPUTERS AND ELECTRONICS IN AGRICULTURE》 *
YANG GAO 等: "Development and calibration of an accurate 6-degree-of-freedom measurement system with total station", 《MEASUREMENT SCIENCE AND TECHNOLOGY》 *
钱尊岩 等: "三维激光扫描技术在城市地下空间建模中的应用", 《北京测绘》 *
陈慎金 等: "基于掘进机位姿测量系统的自主标定方法误差分析", 《煤炭学报》 *

Cited By (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113048972B (zh) * 2021-03-12 2023-05-23 衢州海西电子科技有限公司 矿山工程机械姿态位置确定方法及系统
CN113252063A (zh) * 2021-05-25 2021-08-13 中国煤炭科工集团太原研究院有限公司 一种基于全站仪的采掘装备进深测量方法
CN113252043A (zh) * 2021-05-25 2021-08-13 中国煤炭科工集团太原研究院有限公司 一种基于全站仪的掘进装备机身偏差测量方法
CN113340285A (zh) * 2021-06-18 2021-09-03 中铁工程装备集团有限公司 基于协作机械臂的主机械臂末端位姿检测方法及检测装置
CN115218862A (zh) * 2022-06-16 2022-10-21 上海隧道工程有限公司 基于全站仪的盾构施工管片沉降自动监测系统及监测方法
CN115218862B (zh) * 2022-06-16 2024-05-14 上海隧道工程有限公司 基于全站仪的盾构施工管片沉降自动监测系统及监测方法

Also Published As

Publication number Publication date
CN112344937B (zh) 2022-08-16

Similar Documents

Publication Publication Date Title
CN112344937B (zh) 一种基于单棱镜旋转装置的掘进机位姿测量方法及系统
CN109579831B (zh) 矿用悬臂式掘进机可视化辅助导引方法和系统
CN104750115B (zh) 一种移动设备的激光主动式导航系统及导航方法
CN108827264B (zh) 作业台车及其机械臂光学标靶定位装置和定位方法
CN105301598A (zh) 一种带有组合传感器的空间距离激光测量装置及测量方法
CN110132309A (zh) 一种采煤机摇臂惯性/视觉组合定姿装置标定方法
CN110864692A (zh) 掘进机的位姿确定方法
CN111473803A (zh) 一种用于矿用激光靶的标定方法
CN103954220A (zh) 撞桥试验中船体运动状态数字图像测量方法
CN106225779A (zh) 基于三激光标记点图像的掘进机定位系统及定位方法
Chen et al. Multi-sensor fusion method for roadheader pose detection
CN112797978B (zh) 一种掘进机的导向方法、系统和存储介质
CN210862714U (zh) 车体导航定位装置及导航系统
CN205038346U (zh) 一种带有组合传感器的空间距离激光测量装置
CN104330078B (zh) 一种基于三点后方交会模型的联合测量方法
CN112857367B (zh) 一种基于机器视觉和惯性导航的掘进机位姿检测方法
CN116182743A (zh) 基于激光雷达的煤矿井下掘进巷道轮廓标示系统
US20240077880A1 (en) Slope location correction method and apparatus, robot and readable storage medium
CN114485614B (zh) 基于双全站仪的采掘设备的导航定位系统及方法
CN108733211A (zh) 追踪系统、其操作方法、控制器、及电脑可读取记录媒体
CN206930293U (zh) 两轮车
JP3247143B2 (ja) 移動体の位置・姿勢測量装置
CN114279405B (zh) 一种油缸根部角度测量装置及双护盾掘进机姿态测量系统
US6449854B1 (en) Universal solar chart shadow plotter and method for plotting a shadow
CN217764945U (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