CN108731672B - 基于双目视觉及惯性导航的采煤机姿态检测系统及方法 - Google Patents

基于双目视觉及惯性导航的采煤机姿态检测系统及方法 Download PDF

Info

Publication number
CN108731672B
CN108731672B CN201810538139.XA CN201810538139A CN108731672B CN 108731672 B CN108731672 B CN 108731672B CN 201810538139 A CN201810538139 A CN 201810538139A CN 108731672 B CN108731672 B CN 108731672B
Authority
CN
China
Prior art keywords
laser
coal mining
mining machine
inertial navigation
attitude
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
CN201810538139.XA
Other languages
English (en)
Other versions
CN108731672A (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.)
China University of Mining and Technology CUMT
Original Assignee
China University of Mining and Technology CUMT
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 China University of Mining and Technology CUMT filed Critical China University of Mining and Technology CUMT
Priority to CN201810538139.XA priority Critical patent/CN108731672B/zh
Publication of CN108731672A publication Critical patent/CN108731672A/zh
Application granted granted Critical
Publication of CN108731672B publication Critical patent/CN108731672B/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/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
    • G01C21/165—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 combined with non-inertial navigation instruments
    • 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)
  • Navigation (AREA)

Abstract

本发明公开了一种基于双目视觉及惯性导航的采煤机姿态检测系统及方法，包括捷联惯导系统、激光标靶、摄像头以及激光指向仪，激光指向仪位于采煤工作面上，激光指向仪发射十字激光照在激光标靶上，利用两个摄像头对激光标靶进行实时拍摄，并基于双目视觉原理对标靶上的十字激光点进行跟踪，采用基于顺序一致性的极线约束算法对标志点进行立体匹配，得到标志点的三维坐标，对采煤机姿态的实时解算；再通过捷联惯性导航系统得到采煤机机身的实时角速度和加速度，计算采煤机的实时姿态角，并利用卡尔曼滤波实现惯性导航和视觉信息的融合，从而得到采煤机的精确姿态角。本发明能有效减小测量采煤机姿态角的误差，提高对采煤机姿态定位误差的修正效果。

Description

基于双目视觉及惯性导航的采煤机姿态检测系统及方法
技术领域
本发明涉及一种采煤机姿态检测系统及方法，具体是一种基于双目视觉及惯性导航的采煤机姿态检测系统及方法。
背景技术
为了实现采矿“三机”的联动，对采煤机的空间位置及姿态进行准确检测，即对采煤机进行空间动态定位具有重要意义。为了实现采煤机位置及姿态检测,有学者提出了采煤机惯性导航定位方法。捷联惯性导航系统是指将陀螺仪和加速度计直接固定在运载体上，利用陀螺仪和加速度计等惯性敏感器件对运行载体三轴角速度和三轴加速度信息进行实时测量，结合运行载体初始惯性信息,通过高速积分获得运动载体的姿态、速度及位置等导航信息。捷联惯性导航系统在工作时不依赖外界信息，也不向外界辐射能量，不易受到干扰破坏，是一种自主式导航系统，具有数据更新率高、数据全面以及短时定位精度高等优点。
但同时，对于纯捷联惯性导航系统，也存在着一定的问题，由于采煤机的姿态信息需要进行积分得到，使其具有短时间精度高，但长时间累积会发散的特点，因此在实际系统长时间运行过程中如无法对其累积的误差进行校正，则无法满足采煤机精确定位的要求。
发明内容
针对上述现有技术存在的问题，本发明提供一种基于双目视觉及惯性导航的采煤机姿态检测系统及方法，能有效减小测量采煤机姿态角的误差，提高对采煤机姿态定位误差的修正效果。
为了实现上述目的，本发明采用的技术方案是：一种基于双目视觉及惯性导航的采煤机姿态检测系统，包括防爆箱、固定支架、摄像头、激光标靶、激光指向仪、捷联惯性导航系统、处理器和供电电源；所述防爆箱和激光标靶固定安装在采煤机机身上，捷联惯性导航系统和处理器安装在防爆箱中，摄像头通过固定支架固定在液压支架上，所述摄像头为两个，激光指向仪位于采煤工作面上，激光指向仪发射十字激光，十字激光照射在激光标靶表面，两个摄像头的拍摄方向朝着激光标靶，用于采集激光标靶的图像。
进一步，所述的处理器包括微处理单元模块、通信模块、报警模块、数据存储模块及隔离电路，微处理单元模块分别与通信模块、报警模块、数据存储模块及隔离电路相连。
进一步，所述处理器中微处理单元模块是DSP芯片。
进一步，所述的供电电源采用12V直流锂电池。
进一步，所述摄像头与固定支架为铰接。
一种基于双目视觉及惯性导航的采煤机姿态检测系统的方法，具体步骤为：
A、激光指向仪发射十字激光，十字激光照在激光标靶上，两个摄像头对激光标靶的十字激光标志点进行拍摄，处理器对拍摄到的激光标靶图像先进行图像二值化处理，然后通过设定阈值的方式检测标志点所处的位置，进而得到标志点的二维图像坐标；基于双目视觉原理，两个摄像头对激光标靶上的十字激光点进行实时检测并跟踪，利用基于顺序一致性的极线约束匹配算法对检测到的标志点进行立体匹配，计算得出标志点的三维坐标，从而得到采煤机的姿态信息，达到对采煤机姿态的实时解算；
B、利用捷联惯性导航系统中的陀螺仪测出的地球自转角速度，加速度计测出的重力加速度，采用解析法初始对准求得采煤机的初始姿态角，将采煤机的初始姿态角转化为四元数后，利用捷联惯性导航系统得到采煤机机身的实时角速度和加速度，并经过圆锥误差补偿后采用姿态更新的三子样算法实时计算采煤机的姿态角；
C、利用卡尔曼滤波实现惯性导航和视觉信息的融合，综合考虑系统的精度和实时性，系统考虑全部状态的7维卡尔曼滤波器如下：
Figure GDA0001751667400000021
Z＝HaX+V
其中X＝[δVE δVN δL δλ φx φy φz]，其中δVE为东向速度误差、δVN为北向速度误差、δL为经度误差、δλ为纬度误差、φx、φy、φz分别为失准角，W和V为系统噪声和观测噪声，分别为零均值的高斯白噪声；
根据建立的状态方程和量测方程，将状态方程和量测方程离散化之后，即能建立惯性导航/视觉的闭环卡尔曼滤波方程，将卡尔曼滤波方程得到的状态进行状态反馈后，即得到精确的采煤机姿态角。
与现有技术相比，本发明通过将惯性导航测得的姿态信息和双目视觉测得的姿态信息相结合的方式，通过对双目视觉采集的图像中检测到的标志点进行立体匹配，保证对采煤机的位置与状态确定的精度；由于捷联惯性导航系统在工作时不依赖外界信息，也不向外界辐射能量，不易受到干扰破坏，是一种自主式导航系统，因此具有数据更新率高、数据全面以及短时定位精度高等优点；同时将双目视觉系统与捷联惯性导航系统进行融合，可以进一步减小采煤机姿态角的误差，提高了对采煤机姿态定位误差的修正效果。
附图说明
图1是本发明的整体结构示意图；
图2是本发明中防爆箱内的布设示意图；
图3是本发明中基于双目数据得到采煤机的姿态的流程图；
图4是本发明中通过捷联惯性导航得到的采煤机姿态的流程图；
图5是本发明中惯性导航和视觉信息的融合运行流程图。
图中，1、采煤机，2、防爆箱，3、液压支架，4、固定支架，5、摄像头，6、激光标靶，7、激光指向仪，8、捷联惯性导航系统，9、处理器，10、供电电源。
具体实施方式
下面将对本发明作进一步说明。
如图1和2所示，一种基于双目视觉及惯性导航的采煤机姿态检测系统，包括防爆箱2、固定支架4、摄像头5、激光标靶6、激光指向仪7、捷联惯性导航系统8、处理器9和供电电源10；所述防爆箱2和激光标靶6固定安装在采煤机1机身上，捷联惯性导航系统8和处理器9安装在防爆箱2中，摄像头5通过固定支架4固定在液压支架3上，所述摄像头为两个，激光指向仪7位于采煤工作面上，激光指向仪7发射十字激光，十字激光照射在激光标靶6表面，两个摄像头5的拍摄方向朝着激光标靶6，用于采集激光标靶6的图像。处理器9用于处理捷联惯性导航系统8的加速度信号和角速度信号，以及摄像头5所拍摄的激光标靶6的图像。
进一步，所述的处理器9包括微处理单元模块、通信模块、报警模块、数据存储模块及隔离电路，微处理单元模块分别与通信模块、报警模块、数据存储模块及隔离电路相连。
进一步，所述处理器9中微处理单元模块是TI公司的DSP芯片。该芯片不仅有很高的代码效率，同时可以大大降低系统成本。
进一步，所述的供电电源10采用12V直流锂电池。用于对捷联惯性导航系统8和处理器9供电。
进一步，所述摄像头5与固定支架4为铰接，能使摄像头5相对固定支架转动，如此设置可以使摄像头5最大限度地看到最广的范围，拍摄效果好、无死角。
如图3至图5所示，一种基于双目视觉及惯性导航的采煤机姿态检测系统的方法，具体步骤为：
A、激光指向仪7发射十字激光，十字激光照在激光标靶6上，两个摄像头5对激光标靶6的十字激光标志点进行拍摄，处理器9对拍摄到的激光标靶图像先进行图像二值化处理，然后通过设定阈值的方式检测标志点所处的位置，进而得到标志点的二维图像坐标；基于双目视觉原理，两个摄像头5对激光标靶6上的十字激光点进行实时检测并跟踪，利用基于顺序一致性的极线约束匹配算法对检测到的标志点进行立体匹配，计算得出标志点的三维坐标，从而得到采煤机1的姿态信息，达到对采煤机1姿态的实时解算；
B、利用捷联惯性导航系统8中的陀螺仪测出的地球自转角速度，加速度计测出的重力加速度，采用解析法初始对准求得采煤机1的初始姿态角，将采煤机1的初始姿态角转化为四元数后，利用捷联惯性导航系统8得到采煤机1机身的实时角速度和加速度，并经过圆锥误差补偿后采用姿态更新的三子样算法实时计算采煤机1的姿态角；
C、利用卡尔曼滤波实现惯性导航和视觉信息的融合，综合考虑系统的精度和实时性，系统考虑全部状态的7维卡尔曼滤波器如下：
Figure GDA0001751667400000041
Z＝HaX+V
其中X＝[δVE δVN δL δλ φx φy φz]，其中δVE为东向速度误差、δVN为北向速度误差、δL为经度误差、δλ为纬度误差、φx、φy、φz分别为失准角，W和V为系统噪声和观测噪声，分别为零均值的高斯白噪声；
式中F和Ha为：
Figure GDA0001751667400000051
Figure GDA0001751667400000052
Figure GDA0001751667400000053
Figure GDA0001751667400000054
Figure GDA0001751667400000055
Figure GDA0001751667400000056
根据建立的状态方程和量测方程，将状态方程和量测方程离散化之后，即能建立惯性导航/视觉的闭环卡尔曼滤波方程，将卡尔曼滤波方程得到的状态进行状态反馈后，即得到精确的采煤机姿态角，具体步骤为：
Ⅰ、相关变量初始化，Kalman滤波器初始化；
Ⅱ、开始导航解算，对状态方程进行离散化处理，计算出系统状态转移矩阵；
Figure GDA0001751667400000061
Ⅲ、到达卡尔曼滤波周期时，进行时间更新；
Figure GDA0001751667400000062
Ⅳ、利用两个摄像头对激光标靶的十字激光标志点进行拍摄，对采煤机的姿态角进行实时求解；
Ⅴ、利用惯性导航数据对采煤机的实时姿态、速度、位置进行求解；
Ⅶ、将惯导求得的采煤机姿态角与利用双目视觉求得的采煤机姿态角作差值，其差值作为卡尔曼滤波器的量测输入；
Ⅷ、求解最优估计状态向量
Figure GDA0001751667400000063
和最优估计协方差矩阵Pk+1
Figure GDA0001751667400000064
最终得出精确的采煤机姿态角。

Claims (5)

1.一种基于双目视觉及惯性导航的采煤机姿态检测系统的工作方法，其特征在于，采用的采煤机姿态检测系统包括防爆箱(2)、固定支架(4)、摄像头(5)、激光标靶(6)、激光指向仪(7)、捷联惯性导航系统(8)、处理器(9)和供电电源(10)；所述防爆箱(2)和激光标靶(6)固定安装在采煤机(1)机身上，捷联惯性导航系统(8)和处理器(9)安装在防爆箱(2)中，摄像头(5)通过固定支架(4)固定在液压支架(3)上，所述摄像头为两个，激光指向仪(7)位于采煤工作面上，激光指向仪(7)发射十字激光，十字激光照射在激光标靶(6)表面，两个摄像头(5)的拍摄方向朝着激光标靶(6)，用于采集激光标靶(6)的图像，具体步骤为：
A、激光指向仪(7)发射十字激光，十字激光照在激光标靶(6)上，两个摄像头(5)对激光标靶(6)的十字激光标志点进行拍摄，处理器(9)对拍摄到的激光标靶图像先进行图像二值化处理，然后通过设定阈值的方式检测标志点所处的位置，进而得到标志点的二维图像坐标；基于双目视觉原理，两个摄像头(5)对激光标靶(6)上的十字激光点进行实时检测并跟踪，利用基于顺序一致性的极线约束匹配算法对检测到的标志点进行立体匹配，计算得出标志点的三维坐标，从而得到采煤机(1)的姿态信息，达到对采煤机(1)姿态的实时解算；
B、利用捷联惯性导航系统(8)中的陀螺仪测出的地球自转角速度，加速度计测出的重力加速度，采用解析法初始对准求得采煤机(1)的初始姿态角，将采煤机(1)的初始姿态角转化为四元数后，利用捷联惯性导航系统(8)得到采煤机(1)机身的实时角速度和加速度，并经过圆锥误差补偿后采用姿态更新的三子样算法实时计算采煤机(1)的姿态角；
C、利用卡尔曼滤波实现惯性导航和视觉信息的融合，综合考虑系统的精度和实时性，系统考虑全部状态的7维卡尔曼滤波器如下：
Figure FDA0003202735360000011
Z＝HaX+V
其中x＝[δVE δVN δL δλ φx φy φz]，其中δVE为东向速度误差、δVN为北向速度误差、δL为经度误差、δλ为纬度误差、φx、φy、φz分别为失准角，W和V为系统噪声和观测噪声，分别为零均值的高斯白噪声；
式中F和Ha为：
Figure FDA0003202735360000021
Figure FDA0003202735360000022
Figure FDA0003202735360000023
Figure FDA0003202735360000024
Figure FDA0003202735360000025
Figure FDA0003202735360000026
根据建立的状态方程和量测方程，将状态方程和量测方程离散化之后，即能建立惯性导航/视觉的闭环卡尔曼滤波方程，将卡尔曼滤波方程得到的状态进行状态反馈后，即得到精确的采煤机(1)姿态角。
2.根据权利要求1所述的基于双目视觉及惯性导航的采煤机姿态检测系统的工作方法，其特征在于，所述的处理器(9)包括微处理单元模块、通信模块、报警模块、数据存储模块及隔离电路，微处理单元模块分别与通信模块、报警模块、数据存储模块及隔离电路相连。
3.根据权利要求2所述的基于双目视觉及惯性导航的采煤机姿态检测系统的工作方法，其特征在于，所述处理器(9)中微处理单元模块是DSP芯片。
4.根据权利要求2所述的基于双目视觉及惯性导航的采煤机姿态检测系统的工作方法，其特征在于，所述的供电电源(10)采用12V直流锂电池。
5.根据权利要求2所述的基于双目视觉及惯性导航的采煤机姿态检测系统的工作方法，其特征在于，所述摄像头(5)与固定支架(4)为铰接。
CN201810538139.XA 2018-05-30 2018-05-30 基于双目视觉及惯性导航的采煤机姿态检测系统及方法 Active CN108731672B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201810538139.XA CN108731672B (zh) 2018-05-30 2018-05-30 基于双目视觉及惯性导航的采煤机姿态检测系统及方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201810538139.XA CN108731672B (zh) 2018-05-30 2018-05-30 基于双目视觉及惯性导航的采煤机姿态检测系统及方法

Publications (2)

Publication Number Publication Date
CN108731672A CN108731672A (zh) 2018-11-02
CN108731672B true CN108731672B (zh) 2022-03-11

Family

ID=63936743

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201810538139.XA Active CN108731672B (zh) 2018-05-30 2018-05-30 基于双目视觉及惯性导航的采煤机姿态检测系统及方法

Country Status (1)

Country Link
CN (1) CN108731672B (zh)

Families Citing this family (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109465832A (zh) * 2018-12-18 2019-03-15 哈尔滨工业大学（深圳） 高精度视觉和imu紧融合定位方法与系统
CN109839109B (zh) * 2019-02-25 2020-11-24 中国矿业大学 基于图像识别和多传感器融合的掘进机绝对位姿检测方法
CN110411443B (zh) * 2019-06-05 2023-05-02 西京学院 一种采煤机摇臂惯性/视觉组合定姿装置和方法
CN110132309B (zh) * 2019-06-05 2023-04-25 西京学院 一种采煤机摇臂惯性/视觉组合定姿装置标定方法
CN110231626B (zh) * 2019-06-10 2023-04-18 中国矿业大学 一种采煤机定位监测系统及其监测方法
CN111197982B (zh) * 2020-01-10 2022-04-12 北京航天众信科技有限公司 基于视觉和捷联惯导的掘进机位姿纠偏方法、系统及终端
CN112033400B (zh) * 2020-09-10 2023-07-18 西安科技大学 一种基于捷联惯导与视觉组合的煤矿移动机器人智能定位方法及系统
CN112161571B (zh) * 2020-10-09 2022-02-22 山东科技大学 一种低数据量的双目视觉采煤机定位与位姿检测系统及方法
CN115574843A (zh) * 2022-10-28 2023-01-06 中煤科工集团上海有限公司 采煤机惯性导航精度测评系统及测评方法、移动载体

Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN105371871A (zh) * 2015-12-02 2016-03-02 中国矿业大学 井下采煤机捷联惯导系统的组合初始对准系统及对准方法
CN107063246A (zh) * 2017-04-24 2017-08-18 齐鲁工业大学 一种视觉导航/惯性导航的松散组合导航方法

Family Cites Families (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US8265817B2 (en) * 2008-07-10 2012-09-11 Lockheed Martin Corporation Inertial measurement with an imaging sensor and a digitized map

Patent Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN105371871A (zh) * 2015-12-02 2016-03-02 中国矿业大学 井下采煤机捷联惯导系统的组合初始对准系统及对准方法
CN107063246A (zh) * 2017-04-24 2017-08-18 齐鲁工业大学 一种视觉导航/惯性导航的松散组合导航方法

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
悬臂式掘进机导航技术现状及其发展方向;田原;《工矿自动化》;20170831;第43卷(第8期);40-42 *

Also Published As

Publication number Publication date
CN108731672A (zh) 2018-11-02

Similar Documents

Publication Publication Date Title
CN108731672B (zh) 基于双目视觉及惯性导航的采煤机姿态检测系统及方法
CN110262546B (zh) 一种隧道智能无人机巡检方法
Li et al. UWB-based localization system aided with inertial sensor for underground coal mine applications
CN106017463B (zh) 一种基于定位传感装置的飞行器定位方法
CN109885080B (zh) 自主控制系统及自主控制方法
EP2434256B1 (en) Camera and inertial measurement unit integration with navigation data feedback for feature tracking
CN110487267B (zh) 一种基于vio＆uwb松组合的无人机导航系统及方法
US8903576B2 (en) Device, program product and computer implemented method for touchless metrology using an inertial navigation system and laser
Kelly et al. Combined visual and inertial navigation for an unmanned aerial vehicle
Shen et al. Optical Flow Sensor/INS/Magnetometer Integrated Navigation System for MAV in GPS‐Denied Environment
EP3734394A1 (en) Sensor fusion using inertial and image sensors
US10401175B2 (en) Optical inertial measurement apparatus and method
US10295651B2 (en) Linear optical sensor arrays (LOSA) tracking system for active marker based 3D motion tracking
CN114608554B (zh) 一种手持slam设备以及机器人即时定位与建图方法
CN113140040A (zh) 一种多传感器融合的煤矿井下空间定位及建图方法与装置
US20180075609A1 (en) Method of Estimating Relative Motion Using a Visual-Inertial Sensor
CN116295511B (zh) 一种用于管道潜航机器人的鲁棒初始对准方法及系统
CN112967392A (zh) 一种基于多传感器触合的大规模园区建图定位方法
CN110864692A (zh) 掘进机的位姿确定方法
JP2014240266A (ja) センサドリフト量推定装置及びプログラム
Xian et al. Fusing stereo camera and low-cost inertial measurement unit for autonomous navigation in a tightly-coupled approach
US20170343355A1 (en) Method And System For Estimating Relative Angle Between Headings
Wang et al. Micro aerial vehicle navigation with visual-inertial integration aided by structured light
CN116027351A (zh) 一种手持/背包式slam装置及定位方法
CN115574816A (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