CN103292807B - 基于单目视觉的钻车姿态测量方法 - Google Patents

基于单目视觉的钻车姿态测量方法 Download PDF

Info

Publication number
CN103292807B
CN103292807B CN201210052435.1A CN201210052435A CN103292807B CN 103292807 B CN103292807 B CN 103292807B CN 201210052435 A CN201210052435 A CN 201210052435A CN 103292807 B CN103292807 B CN 103292807B
Authority
CN
China
Prior art keywords
axis
coordinate system
luminous point
attitude
drill carriage
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.)
Expired - Fee Related
Application number
CN201210052435.1A
Other languages
English (en)
Other versions
CN103292807A (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.)
JIANGYIN ZHONGKE MINING SAFETY TECHNOLOGY Co Ltd
Original Assignee
JIANGYIN ZHONGKE MINING SAFETY TECHNOLOGY Co Ltd
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 JIANGYIN ZHONGKE MINING SAFETY TECHNOLOGY Co Ltd filed Critical JIANGYIN ZHONGKE MINING SAFETY TECHNOLOGY Co Ltd
Priority to CN201210052435.1A priority Critical patent/CN103292807B/zh
Publication of CN103292807A publication Critical patent/CN103292807A/zh
Application granted granted Critical
Publication of CN103292807B publication Critical patent/CN103292807B/zh
Expired - Fee Related legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Abstract

本发明涉及一种基于单目视觉的钻车姿态测量方法，利用摄像机采集光点图像，利用矿用工业计算机处理该光点图像，以光点的图像坐标作为特征点，利用摄像机内参数和公知的正交迭代方法求解P4P问题，计算出光点矩形相对于摄像机的姿态，并结合光点矩形相对于钻车的姿态、摄像机相对于辅助平台的姿态、辅助平台相对于地理坐标系的姿态，通过坐标变换计算出钻车相对于地理坐标系的姿态。本发明能够对钻车姿态进行非接触式测量，避免钻车电磁信号对姿态传感器的干扰，所使用的测量方法算法成熟，可靠性高，测量精度高。

Description

基于单目视觉的钻车姿态测量方法
技术领域
本发明涉及一种钻车控制领域中的姿态测量方法，特别涉及一种基于单目视觉的钻车姿态测量方法。
背景技术
煤矿用液压掘进钻车工作时需要明确钻杆的姿态以便实现定向钻进。在实际操作过程中，通常使用价格昂贵的随钻测量系统测量钻杆姿态。在多数非精确钻进应用中，只需要已知钻车自身姿态，然后通过人工矫正的方式即可实现定向钻进。此方案与随钻测量系统相比，具有成本低、可靠性高等特点。目前尚未查阅到与此方案类似的专利和产品。
测量姿态通常使用带有三轴加速度计和三轴磁强计的姿态传感器。但由于这类传感器容易受到电磁干扰的影响，而钻车自身具有强电磁干扰，因此传统的姿态传感器不能直接安装在钻车上使用。必须使传感器与钻车相隔一定距离，以便降低钻车的电磁干扰信号对传感器的影响。
发明内容
本发明的目的在于克服上述不足，提供一种基于单目视觉的钻车姿态测量方法，避免传感器受到钻车电磁干扰，降低测量成本。
本发明的目的是这样实现的：一种基于单目视觉的钻车姿态测量方法，该测量系统包括三轴加速度计、三轴磁强计、摄像机、辅助平台、矿用工业计算机、钻车以及由光点I、光点II、光点III与光点IV组成的光点矩形，所述测量方法包括以下步骤：
步骤一：建立地理坐标系，以辅助平台附近某点为坐标原点，分别使地理坐标系的X轴、Y轴和Z轴指向地理东方、地理北方和地心；
建立摄像机坐标系，以摄像机的光心作为坐标系原点，以摄像机光轴向外方向为Z轴正向，以成像平面水平向左方法为X轴正向，Y轴正方向通过右手法则确定；
建立辅助平台坐标系，以辅助平台的表面中心为原点，以水平向右为X轴正方向，以辅助平台截面向里为Y轴正方向，Z轴正方向通过右手法则确定；
建立光点矩形坐标系，以光点矩形的中心为原点，以光点矩形水平向右为X轴正方向，光点矩形水平向下为Y轴正方向，Z轴正方向通过右手法则确定；
建立钻车坐标系，以钻车的表面某一点为原点，以钻车水平向前为X轴正方向，水平向右为Y轴正方向，Z轴正方向通过右手法则确定；
步骤二：设置摄像机内参数，测量光点矩形的长度和宽度；
步骤三：利用三坐标仪标定摄像机坐标系与辅助平台坐标系的相对姿态R1，光点矩形坐标系与钻车坐标系的相对姿态R2；
步骤四：利用摄像机采集光点图像，以光点图像坐标作为特征点，利用正交迭代方法和步骤二中获得的摄像机内参数求解P4P问题，计算得到光点矩形坐标系相对于摄像机坐标系的姿态R3；
步骤五：利用姿态传感器测量辅助平台坐标系相对于地理坐标系的姿态R4，计算R4包括以下步骤：
5.1、利用三轴加速度计测量辅助平台所处位置的重力加速度矢量[gx,gy,gz]T，其中gx,gy和gz分别为三轴加速度计的输出值；
5.2、利用三轴磁强计测量辅助平台所处位置的磁场强度[mx,my,mz]，其中mx,my和mz分别为三轴磁强计的输出值；
5.3、利用下式计算R4：
其中，
g为重力加速度值。
步骤六：通过坐标变换计算钻车坐标系相对于地理坐标系的姿态R：
R=R2 TR3R1R4
其中，R2 T为姿态R2的转置矩阵。
与现有技术相比，本发明的有益效果：
本发明一种基于单目视觉的钻车姿态测量方法，能够对钻车姿态进行非接触式测量，避免钻车电磁信号对姿态传感器的干扰，系统使用的摄像机、矿用工业计算机以及姿态传感器安装方便，维护方便，造价较低，所使用到的姿态测量方法算法成熟，可靠性高，测量精度高。
附图说明
图1为本发明的钻车姿态测量系统示意图。
图2为本发明的钻车姿态测量系统的坐标系统示意图。
其中：
地理坐标系1
三轴加速度计2
三轴磁强计3
摄像机坐标系4
摄像机5
辅助平台坐标系6
辅助平台7
矿用工业计算机8
光点矩形坐标系9
光点矩形10光点I11光点II12
光点III13光点IV14
钻车坐标系15
钻车16。
具体实施方式
本发明涉及一种基于单目视觉的钻车姿态测量方法。测量系统结构参见图1、图2所示，包括三轴加速度计2、三轴磁强计3、摄像机5、辅助平台7、矿用工业计算机8、光点矩形10、光点I11、光点II12、光点III13、光点IV14和钻车16。
三轴加速度计2采用ADI公司ADXL系列产品，三轴磁强计3采用HONEYWELL公司HMC系列磁阻式磁强计，摄像机5采用防爆摄像机，辅助平台7采用防爆机壳，矿用工业计算机8采用工业防爆计算机，光点I11、光点II12、光点III13和光点IV14均使用高亮度发光二极管。
三轴加速度计2和三轴磁强计3均安装在辅助平台7上，摄像机5固定在辅助平台7上方，其视野能够完整的覆盖光点矩形10，矿用工业计算机8中安装图像采集卡，摄像机5输出的模拟信号通过图像采集卡输入到矿用工业计算机8中。光点I11、光点II12、光点III13和光点IV14通过安装在钻车上的干电池供电发光。
本发明涉及的一种基于单目视觉的钻车姿态测量方法，所述测量方法包括以下步骤：
步骤一：建立地理坐标系1，以辅助平台7附近某点为坐标原点，分别使地理坐标系1的X轴、Y轴和Z轴指向地理东方、地理北方和地心；
建立摄像机坐标系4，以摄像机5的光心作为坐标系原点，以摄像机5光轴向外方向为Z轴正向，以成像平面水平向左方法为X轴正向，Y轴正方向通过右手法则确定；
建立辅助平台坐标系6，以辅助平台7的表面中心为原点，以水平向右为X轴正方向，以辅助平台7截面向里为Y轴正方向，Z轴正方向通过右手法则确定；
建立光点矩形坐标系9，以光点矩形10的中心为原点，以光点矩形10水平向右为X轴正方向，光点矩形10水平向下为Y轴正方向，Z轴正方向通过右手法则确定；
建立钻车坐标系15，以钻车16的表面某一点为原点，以钻车16水平向前为X轴正方向，水平向右为Y轴正方向，Z轴正方向通过右手法则确定。
步骤二：设置摄像机5的内参数，测量光点矩形10的长度和宽度。
步骤三：利用三坐标仪标定摄像机坐标系4与辅助平台坐标系6的相对姿态R1，光点矩形坐标系9与钻车坐标系15的相对姿态R2。
步骤四：利用摄像机5采集光点图像，以光点图像坐标作为特征点，利用正交迭代方法和步骤二中获得的摄像机内参数求解P4P问题，计算得到光点矩形坐标系9相对于摄像机坐标系4的姿态R3；
步骤五：利用姿态传感器测量辅助平台坐标系6相对于地理坐标系1的姿态R4，具体步骤如下：
5.1、利用三轴加速度计2测量辅助平台7所处位置的重力加速度矢量[gx,gy,gz]T，其中gx,gy和gz分别为三轴加速度计2的输出值；
5.2、利用三轴磁强计3测量辅助平台7所处位置的磁场强度[mx,my,mz]，其中mx,my和mz分别为三轴磁强计3的输出值。
5.3、：利用下式计算R4：
其中，
g为重力加速度值。
步骤六：通过坐标变换计算钻车坐标系15相对于地理坐标系1的姿态R：
R=R2 TR3R1R4
其中，R2 T为姿态R2的转置矩阵。
以上所述，仅为本发明中的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉该技术的人在本发明所揭露的技术范围内，可理解或想到的变换或替换，都应涵盖在本发明的包含范围之内。因此，本发明的保护范围应该以权力要求书的保护范围为准。

Claims (1)

1.一种基于单目视觉的钻车姿态测量方法，其特征在于该测量系统包括三轴加速度计（2）、三轴磁强计（3）、摄像机（5）、辅助平台（7）、矿用工业计算机（8）、钻车（16）以及由光点I（11）、光点II（12）、光点III（13）与光点IV（14）组成的光点矩形（10），所述测量方法包括以下步骤：
步骤一：建立地理坐标系（1），以辅助平台（7）附近某点为坐标原点，分别使地理坐标系（1）的X轴、Y轴和Z轴指向地理东方、地理北方和地心；
建立摄像机坐标系（4），以摄像机（5）的光心作为坐标系原点，以摄像机（5）光轴向外方向为Z轴正向，以成像平面水平向左方向为X轴正向，Y轴正方向通过右手法则确定；
建立辅助平台坐标系（6），以辅助平台（7）的表面中心为原点，以水平向右为X轴正方向，以辅助平台7截面向里为Y轴正方向，Z轴正方向通过右手法则确定；
建立光点矩形坐标系（9），以光点矩形（10）的中心为原点，以光点矩形（10）水平向右为X轴正方向，光点矩形（10）水平向下为Y轴正方向，Z轴正方向通过右手法则确定；
建立钻车坐标系（15），以钻车（16）的表面某一点为原点，以钻车（16）水平向前为X轴正方向，水平向右为Y轴正方向，Z轴正方向通过右手法则确定；
步骤二：设置摄像机（5）内参数，测量光点矩形（10）的长度和宽度；
步骤三：利用三坐标仪标定摄像机坐标系（4）与辅助平台坐标系（6）的相对姿态R1，光点矩形坐标系（9）与钻车坐标系（15）的相对姿态R2；
步骤四：利用摄像机（5）采集光点图像，以光点图像坐标作为特征点，利用正交迭代方法和步骤二中获得的摄像机内参数求解P4P问题，计算得到光点矩形坐标系（9）相对于摄像机坐标系（4）的姿态R3；
步骤五：利用姿态传感器测量辅助平台坐标系（6）相对于地理坐标系（1）的姿态R4，计算R4包括以下步骤：
5.1、利用三轴加速度计（2）测量辅助平台（7）所处位置的重力加速度矢量[gx,gy,gz]T，其中gx,gy和gz分别为三轴加速度计（2）的输出值；
5.2、利用三轴磁强计（3）测量辅助平台（7）所处位置的磁场强度[mx,my,mz]，其中mx,my和mz分别为三轴磁强计（3）的输出值；
5.3、利用下式计算R4：
其中，
g为重力加速度值；
步骤六：通过坐标变换计算钻车坐标系（15）相对于地理坐标系（1）的姿态R：
R=R2 TR3R1R4
其中，R2 T为姿态R2的转置矩阵。
CN201210052435.1A 2012-03-02 2012-03-02 基于单目视觉的钻车姿态测量方法 Expired - Fee Related CN103292807B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201210052435.1A CN103292807B (zh) 2012-03-02 2012-03-02 基于单目视觉的钻车姿态测量方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201210052435.1A CN103292807B (zh) 2012-03-02 2012-03-02 基于单目视觉的钻车姿态测量方法

Publications (2)

Publication Number Publication Date
CN103292807A CN103292807A (zh) 2013-09-11
CN103292807B true CN103292807B (zh) 2016-04-20

Family

ID=49094066

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201210052435.1A Expired - Fee Related CN103292807B (zh) 2012-03-02 2012-03-02 基于单目视觉的钻车姿态测量方法

Country Status (1)

Country Link
CN (1) CN103292807B (zh)

Families Citing this family (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104504868B (zh) * 2015-01-04 2017-05-24 合肥联宝信息技术有限公司 监测终端设备用户不良习惯的装置及方法
CN106437706B (zh) * 2016-12-26 2018-09-28 乐清市华尊电气有限公司 一种轮腿式凿岩机器人
CN111356903A (zh) * 2019-01-25 2020-06-30 深圳市大疆创新科技有限公司 视觉定位方法、装置及系统

Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101067557A (zh) * 2007-07-03 2007-11-07 北京控制工程研究所 适用于自主移动车辆的环境感知的单目视觉导航方法
CN102221358A (zh) * 2011-03-23 2011-10-19 中国人民解放军国防科学技术大学 基于逆透视投影变换的单目视觉定位方法
CN102359367A (zh) * 2011-11-01 2012-02-22 上海思萌特电子科技有限公司 一种旋挖钻机监控系统及方法

Family Cites Families (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20060020390A1 (en) * 2004-07-22 2006-01-26 Miller Robert G Method and system for determining change in geologic formations being drilled

Patent Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101067557A (zh) * 2007-07-03 2007-11-07 北京控制工程研究所 适用于自主移动车辆的环境感知的单目视觉导航方法
CN102221358A (zh) * 2011-03-23 2011-10-19 中国人民解放军国防科学技术大学 基于逆透视投影变换的单目视觉定位方法
CN102359367A (zh) * 2011-11-01 2012-02-22 上海思萌特电子科技有限公司 一种旋挖钻机监控系统及方法

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
单目视觉位姿测量方法的研究;肖旭;《中国优秀硕士学位论文全文数据库信息科技辑》;20100830;全文 *

Also Published As

Publication number Publication date
CN103292807A (zh) 2013-09-11

Similar Documents

Publication Publication Date Title
CN110262546B (zh) 一种隧道智能无人机巡检方法
CN102536196B (zh) 基于激光测距和加速度测量的井下姿态测量系统及方法
CA2812062A1 (en) Real-time definitive survey while drilling
CN105091880B (zh) 一种基于标量传感器阵列的追踪定位水下远距离磁性目标的方法
CN103434610B (zh) 一种海上石油钻井平台对接定位引导方法
CN103292807B (zh) 基于单目视觉的钻车姿态测量方法
CN107272069A (zh) 基于磁异常梯度的磁性目标追踪方法
CN103134473A (zh) 无线气压测高装置及测高方法
WO2014059282A3 (en) Attitude reference for tieback/overlap processing
CN109975817A (zh) 一种变电站巡检机器人定位导航方法及系统
CN108168556A (zh) 融合粒子群优化与Taylor级数展开的掘进支护支架超宽带定位方法
CN102954804B (zh) 一种石油钻探测量用传感器正交校准方法
CN103697883B (zh) 一种基于天际线成像的飞行器水平姿态确定方法
CN201310349Y (zh) 一种框架校正式倾斜角方位角测井仪
CN103162632B (zh) 离心模型3d光学位移测量系统
Zeng et al. Measurement method and experiment of hydraulic support group attitude and straightness based on binocular vision
CN210268669U (zh) 一种地面下采掘移动设备用惯性导航定位系统
CN202562485U (zh) 基于单目视觉的钻车姿态测量系统
EP2960682A2 (en) Heading sensor for deflector angle of attack estimation
CN106767670A (zh) 光电吊舱安装偏角辅助标校装置
CN105241434A (zh) 基于wMPS系统的移动发射站快速定向方法
AU2011336375B2 (en) Robotic heliostat and calibration system and method
CN104019748B (zh) 观测基准失稳补偿式接触网偏移量检测装置
Lu et al. Working pose measurement and quality evaluation of rotary drilling rig based on laser tracker
CN207688875U (zh) 一种基于机器视觉的倾角测量仪

Legal Events

Date Code Title Description
C06 Publication
PB01 Publication
C10 Entry into substantive examination
SE01 Entry into force of request for substantive examination
C14 Grant of patent or utility model
GR01 Patent grant
CF01 Termination of patent right due to non-payment of annual fee
CF01 Termination of patent right due to non-payment of annual fee

Granted publication date: 20160420

Termination date: 20170302