CN112857367B - 一种基于机器视觉和惯性导航的掘进机位姿检测方法 - Google Patents

一种基于机器视觉和惯性导航的掘进机位姿检测方法 Download PDF

Info

Publication number
CN112857367B
CN112857367B CN202110084687.1A CN202110084687A CN112857367B CN 112857367 B CN112857367 B CN 112857367B CN 202110084687 A CN202110084687 A CN 202110084687A CN 112857367 B CN112857367 B CN 112857367B
Authority
CN
China
Prior art keywords
target
heading machine
angle
coordinate system
distance
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
CN202110084687.1A
Other languages
English (en)
Other versions
CN112857367A (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.)
Taiyuan Institute of China Coal Technology and Engineering Group
Shanxi Tiandi Coal Mining Machinery Co Ltd
Original Assignee
Taiyuan Institute of China Coal Technology and Engineering Group
Shanxi Tiandi Coal Mining Machinery 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 Taiyuan Institute of China Coal Technology and Engineering Group, Shanxi Tiandi Coal Mining Machinery Co Ltd filed Critical Taiyuan Institute of China Coal Technology and Engineering Group
Priority to CN202110084687.1A priority Critical patent/CN112857367B/zh
Publication of CN112857367A publication Critical patent/CN112857367A/zh
Application granted granted Critical
Publication of CN112857367B publication Critical patent/CN112857367B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

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

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Automation & Control Theory (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
  • Image Processing (AREA)

Abstract

本发明涉及导航定位领域，尤其涉及一种基于机器视觉和惯性导航的掘进机位姿检测方法。包括以下步骤。S100～惯性系统和工业相机刚性连接，并且固定在掘进机机身顶部；惯性系统用来检测掘进机的姿态。S200～工业相机以视频流方式实时显示巷道顶板信息，控制主机以图片格式保存视频流中的帧序列，对图像进行处理，求出图像中目标的位置和旋转、平移参数，其中，巷道顶板特征是巷道的顶板托盘，呈现正方形。S300～交换机通过以太网与控制主机进行数据传输，控制主机对图像数据进行处理、目标检测与识别、目标跟踪，对惯性系统采集的数据进行数据转换，求出掘进机实时姿态参数。S400～利用组合导航检测掘进机的位置参数增量与实时姿态参数。

Description

一种基于机器视觉和惯性导航的掘进机位姿检测方法
技术领域
本发明涉及导航定位领域，尤其涉及一种基于机器视觉和惯性导航的掘进机位姿检测方法。
背景技术
煤炭资源是我国最重要的一次性能源，随着煤炭开采机械化水平的不断提高，煤矿井下巷道的开采变得格外重要。悬臂式掘进机作为煤矿井下巷道综合掘进最重要的一种采掘机械，广泛应用于各种类型的巷道掘进中，但掘进工作面临着工作环境恶劣，工人劳动强度大，掘进方向可控性差，掘进巷道成型质量差等难题，故智能化和自动化的综掘技术是巷道掘进发展的必备条件。
悬臂式掘进机利用其悬臂回转和俯仰动作控制末端截割部进行煤岩切割，截割过程中依靠机身与巷道底板间的摩擦力与截割部分所受侧向截割阻力相平衡，以保持机身相对静止。侧向截割阻力大于机身与巷道底板间的摩擦力时，机身就会发生侧滑和偏转，截割出的断面轮廓形状与位置就可能与设计参数不同，从而改变巷道走向。
导航系统被应用于军事、工业、煤炭行业等众多领域中。目前导航技术多种多样，如航空航天领域中的MEMS-IMU组合导航系统，军事领域中的GPS/SINS组合导航系统和车载GPS与多传感器的组合导航等，因为惯性导航具有自主性、不对外界辐射和不受周围环境影响，机器视觉定位精度高，所以本专利采用惯性导航和机器视觉的组合导航进行掘进机导航定位。
公号为CN109974715A的发明专利“捷联惯导和光斑识别组合的掘进机自主导航系统及方法”，该专利提出利用电子罗盘系统测量掘进机的偏航角、翻滚角和俯仰角，采用线形光斑识别系统检测掘进机位置信息。主要原理是在巷道后方安装扇面激光发射器，在掘进机机身安装激光探测标靶，巷道后方扇面激光发射出的扇面激光垂直投射到机身上的激光探测标靶上，经过信号转换，单片机通过解算激光信号中所携带的线形光斑位置信息，进一步可以得到掘进机位置信息。此发明属于掘进机自主导航领域。
公号为CN109115173A的发明专利“基于直线定位模型的掘进机机身位姿单目视觉测量方法”，该专利主要通过单目相机采集激光光束图像信息，对图像进行处理，解算摄像机的位姿信息，通过相机坐标系与机身载体坐标系之间的相互转换得到掘进机机身的位姿信息。利用全站仪测量激光指向仪的绝对位姿信息，根据激光指向仪与相机的位置关系，计算相机的绝对位姿信息，通过相机坐标系和掘进机载体坐标系之间的信息传递，得到掘进机的绝对位姿信息。该方法属于机器视觉在掘进机导航定位领域的应用。
发明内容
本发明为了解决上述问题，提供一种基于机器视觉和惯性导航的掘进机位姿检测方法。
本发明采取以下技术方案：一种基于机器视觉和惯性导航的掘进机位姿检测方法，包括以下步骤。
S100～惯性系统和工业相机刚性连接，并且固定在掘进机机身顶部；惯性系统用来检测掘进机的姿态。根据当地的经纬高参数对惯性系统进行初对准，对准结束后开始采集掘进机地理坐标系下的X轴、Y轴和Z轴角度参数，根据掘进机姿态角的定义，求出其姿态角，并传输至交换机。
S200～工业相机以视频流方式实时显示巷道顶板信息，控制主机以图片格式保存视频流中的帧序列，对图像进行处理，求出图像中目标的位置和旋转、平移参数，其中，巷道顶板特征是巷道的顶板托盘，呈现正方形。
S300～交换机通过以太网与控制主机进行数据传输，控制主机对图像数据进行处理、目标检测与识别、目标跟踪，对惯性系统采集的数据进行数据转换，求出掘进机实时姿态参数。
S400～利用组合导航检测掘进机的位置参数增量与实时姿态参数。
步骤S100中，需要根据掘进机地理坐标系下的X轴、Y轴和Z轴角度求出掘进机的姿态角；建立载体坐标系，坐标系原点为掘进机的质心Ob，Xb轴方向为掘进机横轴正右方，Yb轴方向为掘进机纵轴正前方，Zb轴方向正交Xb轴、Yb轴竖直向上。
姿态角的定义：
俯仰角：载体坐标系绕地理坐标系旋转，Yb轴与OnXnYn水平面的夹角，用θ表示，当Yb轴的正半轴位于过坐标原点的水平面之上时，俯仰角为正，反之为负，旋转范围是-90°～90°。
翻滚角：载体坐标系绕地理坐标系旋转，Zb轴与通过载体Yb轴的铅垂面间的夹角，用γ表示。以载体向右倾斜为正，反之为负，旋转范围是-180°～180°。
偏航角：载体坐标系绕地理坐标系旋转，Yb轴在OnXnYn水平面上的投影与Yn之间的夹角，用表示。以载体右偏为正，反之为负，旋转范围是0°～360°。
对应的坐标变换矩阵如下：
偏航角的旋转矩阵为：
俯仰角θ的旋转矩阵为：
翻滚角γ的旋转矩阵为：
三个基本旋转矩阵进行合并，求出载体坐标系与地理坐标系的旋转矩阵：
式中，为单位正交矩阵，有：
载体坐标系与地理坐标系之间的转换可以借助姿态矩阵完成，该姿态矩阵可以简化表示为：
由于地理坐标系到载体坐标系的旋转过程中坐标系始终保持直角坐标系，所以为正交矩阵，则有：
对比式(6)与式(7)中的姿态角和姿态矩阵，可在已知姿态矩阵的情况下反推求出姿态角，则有：
步骤S200中，掘进机行进过程中，工业相机的视野范围内至少覆盖两个目标。
步骤S200具体步骤如下，控制主机接收t0时刻惯性系统采集的地理坐标系下X轴、Y轴和Z轴角度参数，并解算出此时掘进机的偏航角翻滚角γ0和俯仰角θ0；控制主机对t0时刻采集的图像进行处理，采用霍夫变换方法检测图像中每个目标特征的直线，并求出直线的交点，因为本专利所选巷道顶板特征为顶板托盘，呈现正方形，所以每个目标可以求出4个交点，利用PnP算法根据每个目标4个交点的像素坐标求出目标此时的旋转向量rec0＝[r11,r12,r13]T和平移向量T0＝[t11,t12,t13]T；掘进机行进过程中，求出t1时刻掘进机的偏航角/>翻滚角γ1和俯仰角θ1；设定相机和惯性系统的数据采集周期为1s，因为掘进机在煤岩巷道一天行进距离为30～40米，岩巷一天行进距离为3～5米，所以假设在周期t0和t1时间段内其平移向量变化量为零。在t0和t1时间段内掘进机偏航角变化量为/>翻滚角变化量为Δγ＝γ1-γ0，俯仰角变化量为Δθ＝θ1-θ0；巷道顶板特征是固定不变的，其位姿变化量是相对相机的变化量，相机固定在掘进顶部，因此，掘进机的旋转、平移变化量即为巷道顶板特征的旋转、平移变化量。将t0时刻的图像中目标的旋转向量rec0转换为偏航角度/>翻滚角γ′0和俯仰角θ′0,则t1时刻图像中目标的偏航角度为/>翻滚角γ′1＝γ′0+Δγ和俯仰角θ′1＝θ′0+Δθ,将t1时刻图像中目标的旋转角度转换为旋转向量R＝[R11,R12,R13]，根据t1时刻图像中目标旋转向量R和t0刻的平移向量T0利用PnP算法求出t1时刻图像中每个目标的估计位置；控制主机处理t1时刻采集的目标图像，采用霍夫变换方法检测图像中每个目标的直线，并求出直线交点的像素坐标，即目标t1时刻的实际位置。利用中心点匹配算法找到与目标估计位置最近邻的实际位置，即t0时刻的目标在t1时刻图像中的位置；根据t1时刻图像中每个目标的四个交点坐标，采用PnP算法求出每个目标的旋转向量rec1＝[r′11,r′12,r′13]T和平移向量T1＝[t11,t′12,t′13]T，则t0和t1时间段内掘进机的平移向量的增量为相匹配的目标平移向量相减，即ΔT＝T1-T0。
步骤S200具体步骤如下，利用PnP算法求出t1时刻图像中两个目标特征的估计位置，每个目标的像素坐标分别为((x′11,y′11),(x′12,y′12),(x′13,y′13),(x′14,y′14))和((x′21,y′21),(x′22,y′22),(x′23,y′23),(x′24,y′24))，两个目标的中心点估计坐标分别为(x1′center,y1′center)和(x2′center,y2′center),其中，利用PnP算法求出t1时刻图像中两个目标特征的实际位置，每个目标的像素坐标分别为((x11,y11),(x12,y12),(x13,y13),(x14,y14))和((x21,y21),(x22,y22),(x23,y23),(x24,y24))；两个目标的中心点实际坐标分别为(x1center,y1center)和(x2center,y2center)，其中，/>每个目标估计位置与两个目标实际位置的欧氏距离分别为：
其中，distance11为第一个目标估计位置与第一个目标实际位置的欧式距离，distance12为第一个目标估计位置与第二个目标实际位置的欧式距离，distance21为第二个目标估计位置与第一个目标实际位置的欧式距离，distance22为第二个目标估计位置与第二个目标实际位置的欧式距离；如果distance11小于distance12，且distance21大于distance22，则第一个目标的估计位置与第一个实际位置相匹配，第二个目标的估计位置与第二个实际位置相匹配，如果distance11大于distance12，且distance21小于distance22，则第一个目标的估计位置与第二个其实际位置相匹配，第二个目标的估计位置与第一个实际位置相匹配。
与现有技术相比，本发明设计合理，适用性强，稳定性高，可以实现井下掘进机实时的、高精度的位置姿态检测，降低了井下掘进机工作面的作业人数，提高了井下掘进工作面的自动化、智能化集中程度。
附图说明
图1为掘进机位姿检测示意图；
图2为掘进机位姿检测方法框图；
图3为载体坐标系示意图；
图4为X轴旋转示意图；
图5为Y轴旋转示意图；
图6为Z轴旋转示意图；
图中1-巷道顶板，2-目标特征，3-钢带，4-相机，5-惯性系统，6-交换机，7-掘进机，8-控制主机。
具体实施方式
一种基于机器视觉和惯性系统的掘进机位姿检测方法，包括以下步骤，
S100～惯性系统和工业相机刚性连接，并且固定在掘进机机身顶部；惯性系统用来检测掘进机的姿态。根据当地的经纬高参数对惯性系统进行初对准，对准结束后开始采集掘进机地理坐标系下的X轴、Y轴和Z轴角度参数，根据掘进机姿态角的定义，求出其姿态角，并传输至交换机。
S200～工业相机以视频流方式实时显示巷道顶板信息，控制主机以图片格式保存视频流中的帧序列，对图像进行处理，求出图像中目标的位置和旋转、平移参数，其中，巷道顶板特征是巷道的顶板托盘，呈现正方形。
S300～交换机通过以太网与控制主机进行数据传输，控制主机对图像数据进行处理、目标检测与识别、目标跟踪，对惯性系统采集的数据进行数据转换，求出掘进机实时姿态参数。
S400～利用组合导航检测掘进机的位置参数增量与实时姿态参数。
步骤S100中，需要根据掘进机地理坐标系下的X轴、Y轴和Z轴角度求出掘进机的姿态角；建立载体坐标系，坐标系原点为掘进机的质心Ob，Xb轴方向为掘进机横轴正右方，Yb轴方向为掘进机纵轴正前方，Zb轴方向正交Xb轴、Yb轴竖直向上。
姿态角的定义：
俯仰角：载体坐标系绕地理坐标系旋转，Yb轴与OnXnYn水平面的夹角，用θ表示，当Yb轴的正半轴位于过坐标原点的水平面之上时，俯仰角为正，反之为负，旋转范围是-90°～90°。
翻滚角：载体坐标系绕地理坐标系旋转，Zb轴与通过载体Yb轴的铅垂面间的夹角，用γ表示。以载体向右倾斜为正，反之为负，旋转范围是-180°～180°。
偏航角：载体坐标系绕地理坐标系旋转，Yb轴在OnXnYn水平面上的投影与Yn之间的夹角，用表示。以载体右偏为正，反之为负，旋转范围是0°～360°。
对应的坐标变换矩阵如下：
偏航角的旋转矩阵为：
俯仰角θ的旋转矩阵为：
翻滚角γ的旋转矩阵为：
三个基本旋转矩阵进行合并，求出载体坐标系与地理坐标系的旋转矩阵
式中，为单位正交矩阵，有：
载体坐标系与地理坐标系之间的转换可以借助姿态矩阵完成，该姿态矩阵可以简化表示为：
由于地理坐标系到载体坐标系的旋转过程中坐标系始终保持直角坐标系，所以为正交矩阵，则有：
对比式(6)与式(7)中的姿态角和姿态矩阵，可在已知姿态矩阵的情况下反推求出姿态角，则有：
步骤S200中，掘进机行进过程中，工业相机的视野范围内至少覆盖两个目标。
步骤S200具体步骤如下，控制主机接收t0时刻惯性系统采集的地理坐标系下X轴、Y轴和Z轴角度参数，并解算出此时掘进机的偏航角翻滚角γ0和俯仰角θ0；控制主机对t0时刻采集的图像进行处理，采用霍夫变换方法检测图像中每个目标特征的直线，并求出直线的交点，因为本专利所选巷道顶板特征为顶板托盘，呈现正方形，所以每个目标可以求出4个交点，利用PnP算法根据每个目标4个交点的像素坐标求出目标此时的旋转向量rec0＝[r11,r12,r13]T和平移向量T0＝[t11,t12,t13]T；掘进机行进过程中，求出t1时刻掘进机的偏航角/>翻滚角γ1和俯仰角θ1；设定相机和惯性系统的数据采集周期为1s，因为掘进机在煤岩巷道一天行进距离为30～40米，岩巷一天行进距离为3～5米，所以假设在周期t0和t1时间段内其平移向量变化量为零。在t0和t1时间段内掘进机偏航角变化量为/>翻滚角变化量为Δγ＝γ1-γ0，俯仰角变化量为Δθ＝θ1-θ0；巷道顶板特征是固定不变的，其位姿变化量是相对相机的变化量，相机固定在掘进顶部，因此，掘进机的旋转、平移变化量即为巷道顶板特征的旋转、平移变化量。将t0时刻的图像中目标的旋转向量rec0转换为偏航角度/>翻滚角γ′0和俯仰角θ′0,则t1时刻图像中目标的偏航角度为/>翻滚角γ′1＝γ′0+Δγ和俯仰角θ′1＝θ′0+Δθ,将t1时刻图像中目标的旋转角度转换为旋转向量R＝[R11,R12,R13]，根据t1时刻图像中目标旋转向量R和t0刻的平移向量T0利用PnP算法求出t1时刻图像中每个目标的估计位置；控制主机处理t1时刻采集的目标图像，采用霍夫变换方法检测图像中每个目标的直线，并求出直线交点的像素坐标，即目标t1时刻的实际位置。利用中心点匹配算法找到与目标估计位置最近邻的实际位置，即t0时刻的目标在t1时刻图像中的位置；根据t1时刻图像中每个目标的四个交点坐标，采用PnP算法求出每个目标的旋转向量rec1＝[r′11,r′12,r′13]T和平移向量T1＝[t11,t′12,t′13]T，则t0和t1时间段内掘进机的平移向量的增量为相匹配的目标平移向量相减，即ΔT＝T1-T0。
步骤S200具体步骤如下，利用PnP算法求出t1时刻图像中两个目标特征的估计位置，每个目标的像素坐标分别为((x′11,y′11),(x′12,y′12),(x′13,y′13),(x′14,y′14))和((x′21,y′21),(x′22,y′22),(x′23,y′23),(x′24,y′24))，两个目标的中心点估计坐标分别为(x1′center,y1′center)和(x2′center,y2′center),其中， 利用PnP算法求出t1时刻图像中两个目标特征的实际位置，每个目标特征的像素坐标分别为((x11,y11),(x12,y12),(x13,y13),(x14,y14))和((x21,y21),(x22,y22),(x23,y23),(x24,y24))；两个目标的中心点实际坐标分别为(x1center,y1center)和(x2center,y2center)，其中，/> 每个目标估计位置与两个目标实际位置的欧氏距离分别为/> 其中，distance11为第一个目标估计位置与第一个目标实际位置的欧式距离，distance12为第一个目标估计位置与第二个目标实际位置的欧式距离，distance21为第二个目标估计位置与第一个目标实际位置的欧式距离，distance22为第二个目标估计位置与第二个目标实际位置的欧式距离；如果distance11小于distance12，且distance21大于distance22，则第一个目标的估计位置与第一个实际位置相匹配，第二个目标的估计位置与第二个实际位置相匹配，如果distance11大于distance12，且distance21小于distance22，则第一个目标的估计位置与第二个其实际位置相匹配，第二个目标的估计位置与第一个实际位置相匹配。

Claims (3)

1.一种基于机器视觉和惯性系统的掘进机位姿检测方法，其特征在于：包括以下步骤，
S100～惯性系统和工业相机刚性连接，并且固定在掘进机机身顶部；惯性系统用来检测掘进机的姿态，根据当地的经纬高参数对惯性系统进行初对准，对准结束后开始采集掘进机地理坐标系下的X轴、Y轴和Z轴角度参数，根据掘进机姿态角的定义，求出其姿态角，并传输至交换机；
步骤S100中，需要根据掘进机地理坐标系下的X轴、Y轴和Z轴角度求出掘进机的姿态角；建立载体坐标系，坐标系原点为掘进机的质心Ob，Xb轴方向为掘进机横轴正右方，Yb轴方向为掘进机纵轴正前方，Zb轴方向正交Xb轴、Yb轴竖直向上；
姿态角的定义：
俯仰角：载体坐标系绕地理坐标系旋转，Yb轴与OnXnYn水平面的夹角，用θ表示，当Yb轴的正半轴位于过坐标原点的水平面之上时，俯仰角为正，反之为负，旋转范围是-90°～90°；
翻滚角：载体坐标系绕地理坐标系旋转，Zb轴与通过载体Yb轴的铅垂面间的夹角，用γ表示，以载体向右倾斜为正，反之为负，旋转范围是-180°～180°；
偏航角：载体坐标系绕地理坐标系旋转，Yb轴在OnXnYn水平面上的投影与Yn之间的夹角，用表示，以载体右偏为正，反之为负，旋转范围是0°～360°；
对应的坐标变换矩阵如下：
偏航角的旋转矩阵为：
俯仰角θ的旋转矩阵为：
翻滚角γ的旋转矩阵为：
三个基本旋转矩阵进行合并，求出载体坐标系与地理坐标系的旋转矩阵：
式中，为单位正交矩阵，有：
载体坐标系与地理坐标系之间的转换可以借助姿态矩阵完成，该姿态矩阵可以简化表示为：
由于地理坐标系到载体坐标系的旋转过程中坐标系始终保持直角坐标系，所以为正交矩阵，则有：
对比式(6)与式(7)中的姿态角和姿态矩阵，可在已知姿态矩阵的情况下反推求出姿态角，则有：
S200～工业相机以视频流方式实时显示巷道顶板信息，控制主机以图片格式保存视频流中的帧序列，对图像进行处理，求出图像中目标的位置和旋转、平移参数，其中，巷道顶板特征是巷道的顶板托盘，呈现正方形；
步骤S200具体步骤如下，控制主机接收t0时刻惯性系统采集的地理坐标系下X轴、Y轴和Z轴角度参数，并解算出此时掘进机的偏航角翻滚角γ0和俯仰角θ0；控制主机对t0时刻采集的图像进行处理，采用霍夫变换方法检测图像中每个目标特征的直线，并求出直线的交点，因为本专利所选巷道顶板特征为顶板托盘，呈现正方形，所以每个目标可以求出4个交点，利用PnP算法根据每个目标4个交点的像素坐标求出目标此时的旋转向量rec0＝[r11,r12,r13]T和平移向量T0＝[t11,t12,t13]T；掘进机行进过程中，求出t1时刻掘进机的偏航角翻滚角γ1和俯仰角θ1；设定相机和惯性系统的数据采集周期为1s，因为掘进机在煤岩巷道一天行进距离为30～40米，岩巷一天行进距离为3～5米，所以假设在周期t0和t1时间段内其平移向量变化量为零；在t0和t1时间段内掘进机偏航角变化量为/>翻滚角变化量为Δγ＝γ1-γ0，俯仰角变化量为Δθ＝θ1-θ0；巷道顶板特征是固定不变的，其位姿变化量是相对相机的变化量，相机固定在掘进顶部，因此，掘进机的旋转、平移变化量即为巷道顶板特征的旋转、平移变化量；将t0时刻的图像中目标的旋转向量rec0转换为偏航角度翻滚角γ′0和俯仰角θ′0,则t1时刻图像中目标的偏航角度为/>翻滚角γ′1＝γ′0+Δγ和俯仰角θ′1＝θ′0+Δθ,将t1时刻图像中目标的旋转角度转换为旋转向量R＝[R11,R12,R13]，根据t1时刻图像中目标旋转向量R和t0刻的平移向量T0利用PnP算法求出t1时刻图像中每个目标的估计位置；控制主机处理t1时刻采集的目标图像，采用霍夫变换方法检测图像中每个目标的直线，并求出直线交点的像素坐标，即目标t1时刻的实际位置；利用中心点匹配算法找到与目标估计位置最近邻的实际位置，即t0时刻的目标在t1时刻图像中的位置；根据t1时刻图像中每个目标的四个交点坐标，采用PnP算法求出每个目标的旋转向量rec1＝[r′11,r′12,r′13]T和平移向量T1＝[t11,t′12,t′13]T，则t0和t1时间段内掘进机的平移向量的增量为相匹配的目标平移向量相减，即ΔT＝T1-T0；
S300～交换机通过以太网与控制主机进行数据传输，控制主机对图像数据进行处理、目标检测与识别、目标跟踪，对惯性系统采集的数据进行数据转换，求出掘进机实时姿态参数；
S400～利用组合导航检测掘进机的位置参数增量与实时姿态参数。
2.根据权利要求1所述的机器视觉和惯性系统的掘进机位姿检测方法，其特征在于：所述的步骤S200中，掘进机行进过程中，工业相机的视野范围内至少覆盖两个目标。
3.根据权利要求2所述的机器视觉和惯性系统的掘进机位姿检测方法，其特征在于：所述的步骤S400具体步骤如下，利用PnP算法求出t1时刻图像中两个目标特征的估计位置，每个目标的像素坐标分别为((x′11,y′11),(x′12,y′12),(x′13,y′13),(x′14,y′14))和((x′21,y′21),(x′22,y′22),(x′23,y′23),(x′24,y′24))，两个目标的中心点估计坐标分别为(x1′center,y1′center)和(x2′center,y2′center),其中， 利用PnP算法求出t1时刻图像中两个目标特征的实际位置，每个目标特征的像素坐标分别为((x11,y11),(x12,y12),(x13,y13),(x14,y14))和((x21,y21),(x22,y22),(x23,y23),(x24,y24))；两个目标的中心点实际坐标分别为(x1center,y1center)和(x2center,y2center)，其中， 每个目标估计位置与两个目标实际位置的欧氏距离分别为/> 其中，distance11为第一个目标估计位置与第一个目标实际位置的欧式距离，distance12为第一个目标估计位置与第二个目标实际位置的欧式距离，distance21为第二个目标估计位置与第一个目标实际位置的欧式距离，distance22为第二个目标估计位置与第二个目标实际位置的欧式距离；如果distance11小于distance12，且distance21大于distance22，则第一个目标的估计位置与第一个实际位置相匹配，第二个目标的估计位置与第二个实际位置相匹配，如果distance11大于distance12，且distance21小于distance22，则第一个目标的估计位置与第二个其实际位置相匹配，第二个目标的估计位置与第一个实际位置相匹配。
CN202110084687.1A 2021-01-21 2021-01-21 一种基于机器视觉和惯性导航的掘进机位姿检测方法 Active CN112857367B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110084687.1A CN112857367B (zh) 2021-01-21 2021-01-21 一种基于机器视觉和惯性导航的掘进机位姿检测方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110084687.1A CN112857367B (zh) 2021-01-21 2021-01-21 一种基于机器视觉和惯性导航的掘进机位姿检测方法

Publications (2)

Publication Number Publication Date
CN112857367A CN112857367A (zh) 2021-05-28
CN112857367B true CN112857367B (zh) 2023-10-13

Family

ID=76008988

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110084687.1A Active CN112857367B (zh) 2021-01-21 2021-01-21 一种基于机器视觉和惯性导航的掘进机位姿检测方法

Country Status (1)

Country Link
CN (1) CN112857367B (zh)

Families Citing this family (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116295313B (zh) * 2023-05-22 2023-07-18 太原理工大学 掘进机的实时定位系统

Citations (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN105021184A (zh) * 2015-07-08 2015-11-04 西安电子科技大学 一种用于移动平台下视觉着舰导航的位姿估计系统及方法
CN106625673A (zh) * 2016-12-29 2017-05-10 南京天祥智能设备科技有限公司 狭小空间装配系统及装配方法
CN106679648A (zh) * 2016-12-08 2017-05-17 东南大学 一种基于遗传算法的视觉惯性组合的slam方法
WO2017092181A1 (zh) * 2015-12-02 2017-06-08 中国矿业大学 井下采煤机捷联惯导系统的组合初始对准系统及对准方法
CN109115173A (zh) * 2018-08-23 2019-01-01 西安科技大学 基于直线定位模型的掘进机机身位姿单目视觉测量方法
CN109631938A (zh) * 2018-12-28 2019-04-16 湖南海迅自动化技术有限公司 掘进机自主定位定向系统及方法
CN110017817A (zh) * 2019-01-24 2019-07-16 中国煤炭科工集团太原研究院有限公司 一种基于顶板特征的煤矿巷道导航定位方法和装置
CN110095116A (zh) * 2019-04-29 2019-08-06 桂林电子科技大学 一种基于lift的视觉定位和惯性导航组合的定位方法
CN110132272A (zh) * 2019-06-20 2019-08-16 河北工业大学 一种用于空间碎片运动参数的测量方法及系统
CN111207742A (zh) * 2020-01-17 2020-05-29 西安科技大学 一种附加外方位元素约束的采煤机定位定姿方法

Patent Citations (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN105021184A (zh) * 2015-07-08 2015-11-04 西安电子科技大学 一种用于移动平台下视觉着舰导航的位姿估计系统及方法
WO2017092181A1 (zh) * 2015-12-02 2017-06-08 中国矿业大学 井下采煤机捷联惯导系统的组合初始对准系统及对准方法
CN106679648A (zh) * 2016-12-08 2017-05-17 东南大学 一种基于遗传算法的视觉惯性组合的slam方法
CN106625673A (zh) * 2016-12-29 2017-05-10 南京天祥智能设备科技有限公司 狭小空间装配系统及装配方法
CN109115173A (zh) * 2018-08-23 2019-01-01 西安科技大学 基于直线定位模型的掘进机机身位姿单目视觉测量方法
CN109631938A (zh) * 2018-12-28 2019-04-16 湖南海迅自动化技术有限公司 掘进机自主定位定向系统及方法
CN110017817A (zh) * 2019-01-24 2019-07-16 中国煤炭科工集团太原研究院有限公司 一种基于顶板特征的煤矿巷道导航定位方法和装置
CN110095116A (zh) * 2019-04-29 2019-08-06 桂林电子科技大学 一种基于lift的视觉定位和惯性导航组合的定位方法
CN110132272A (zh) * 2019-06-20 2019-08-16 河北工业大学 一种用于空间碎片运动参数的测量方法及系统
CN111207742A (zh) * 2020-01-17 2020-05-29 西安科技大学 一种附加外方位元素约束的采煤机定位定姿方法

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
基于惯性测量的悬臂式掘进机位姿自动定位;呼守信;;机电产品开发与创新(03);全文 *
机器人化掘进机的运动分析及车体定位;李军利;廉自生;李元宗;;煤炭学报(05);全文 *

Also Published As

Publication number Publication date
CN112857367A (zh) 2021-05-28

Similar Documents

Publication Publication Date Title
Alonso et al. Accurate global localization using visual odometry and digital maps on urban environments
CN110160542B (zh) 车道线的定位方法和装置、存储介质、电子装置
CN110208842A (zh) 一种车联网环境下车辆高精度定位方法
CN109115173B (zh) 基于直线定位模型的掘进机机身位姿单目视觉测量方法
CN111426320B (zh) 一种基于图像匹配/惯导/里程计的车辆自主导航方法
CN106338245A (zh) 一种工件非接触移动测量方法
CN110361008B (zh) 地下车库自动泊车的定位方法及装置
CN110160557B (zh) 一种掘进机惯性导航系统二维位置精度标定方法和系统
AU2021350657A1 (en) Positioning method and system for fully mechanized mining face
CN112344937A (zh) 一种基于单棱镜旋转装置的掘进机位姿测量方法及系统
CN112857367B (zh) 一种基于机器视觉和惯性导航的掘进机位姿检测方法
CN114593739B (zh) 基于视觉检测与参考线匹配的车辆全局定位方法及装置
CN106225779A (zh) 基于三激光标记点图像的掘进机定位系统及定位方法
CN110411443B (zh) 一种采煤机摇臂惯性/视觉组合定姿装置和方法
Gao et al. MGG: Monocular global geolocation for outdoor long-range targets
Le et al. Vehicle localization using omnidirectional camera with GPS supporting in wide urban area
CN109670204B (zh) 运载火箭视频图像判读误差修正方法
Deusch et al. Improving localization in digital maps with grid maps
CN116704019A (zh) 一种基于锚杆网络的钻锚机器人单目视觉定位方法
US20240077880A1 (en) Slope location correction method and apparatus, robot and readable storage medium
Ye et al. A vision-based guidance method for autonomous guided vehicles
CN113670318B (zh) 协同定位方法及定位系统
CN116626686A (zh) 一种掘进机定位定向方法、系统、设备及存储介质
Xu et al. Research on orb-slam autonomous navigation algorithm
CN113341968A (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