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

Abstract

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

Description

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

技术领域

本发明涉及导航定位领域，尤其涉及一种基于机器视觉和惯性导航的掘进机位姿检测方法。

背景技术

煤炭资源是我国最重要的一次性能源，随着煤炭开采机械化水平的不断提高，煤矿井下巷道的开采变得格外重要。悬臂式掘进机作为煤矿井下巷道综合掘进最重要的一种采掘机械，广泛应用于各种类型的巷道掘进中，但掘进工作面临着工作环境恶劣，工人劳动强度大，掘进方向可控性差，掘进巷道成型质量差等难题，故智能化和自动化的综掘技术是巷道掘进发展的必备条件。

悬臂式掘进机利用其悬臂回转和俯仰动作控制末端截割部进行煤岩切割，截割过程中依靠机身与巷道底板间的摩擦力与截割部分所受侧向截割阻力相平衡，以保持机身相对静止。侧向截割阻力大于机身与巷道底板间的摩擦力时，机身就会发生侧滑和偏转，截割出的断面轮廓形状与位置就可能与设计参数不同，从而改变巷道走向。

导航系统被应用于军事、工业、煤炭行业等众多领域中。目前导航技术多种多样，如航空航天领域中的MEMS-IMU组合导航系统，军事领域中的GPS/SINS组合导航系统和车载GPS与多传感器的组合导航等，因为惯性导航具有自主性、不对外界辐射和不受周围环境影响，机器视觉定位精度高，所以本专利采用惯性导航和机器视觉的组合导航进行掘进机导航定位。

公号为CN109974715A的发明专利“捷联惯导和光斑识别组合的掘进机自主导航系统及方法”，该专利提出利用电子罗盘系统测量掘进机的偏航角、翻滚角和俯仰角，采用线形光斑识别系统检测掘进机位置信息。主要原理是在巷道后方安装扇面激光发射器，在掘进机机身安装激光探测标靶，巷道后方扇面激光发射出的扇面激光垂直投射到机身上的激光探测标靶上，经过信号转换，单片机通过解算激光信号中所携带的线形光斑位置信息，进一步可以得到掘进机位置信息。此发明属于掘进机自主导航领域。

公号为CN109115173A的发明专利“基于直线定位模型的掘进机机身位姿单目视觉测量方法”，该专利主要通过单目相机采集激光光束图像信息，对图像进行处理，解算摄像机的位姿信息，通过相机坐标系与机身载体坐标系之间的相互转换得到掘进机机身的位姿信息。利用全站仪测量激光指向仪的绝对位姿信息，根据激光指向仪与相机的位置关系，计算相机的绝对位姿信息，通过相机坐标系和掘进机载体坐标系之间的信息传递，得到掘进机的绝对位姿信息。该方法属于机器视觉在掘进机导航定位领域的应用。

发明内容

本发明为了解决上述问题，提供一种基于机器视觉和惯性导航的掘进机位姿检测方法。

本发明采取以下技术方案：一种基于机器视觉和惯性导航的掘进机位姿检测方法，包括以下步骤。

S100～惯性系统和工业相机刚性连接，并且固定在掘进机机身顶部；惯性系统用来检测掘进机的姿态。根据当地的经纬高参数对惯性系统进行初对准，对准结束后开始采集掘进机地理坐标系下的X轴、Y轴和Z轴角度参数，根据掘进机姿态角的定义，求出其姿态角，并传输至交换机。

S200～工业相机以视频流方式实时显示巷道顶板信息，控制主机以图片格式保存视频流中的帧序列，对图像进行处理，求出图像中目标的位置和旋转、平移参数，其中，巷道顶板特征是巷道的顶板托盘，呈现正方形。

S300～交换机通过以太网与控制主机进行数据传输，控制主机对图像数据进行处理、目标检测与识别、目标跟踪，对惯性系统采集的数据进行数据转换，求出掘进机实时姿态参数。

S400～利用组合导航检测掘进机的位置参数增量与实时姿态参数。

步骤S100中，需要根据掘进机地理坐标系下的X轴、Y轴和Z轴角度求出掘进机的姿态角；建立载体坐标系，坐标系原点为掘进机的质心O_b，X_b轴方向为掘进机横轴正右方，Y_b轴方向为掘进机纵轴正前方，Z_b轴方向正交X_b轴、Y_b轴竖直向上。

姿态角的定义：

俯仰角：载体坐标系绕地理坐标系旋转，Y_b轴与O_nX_nY_n水平面的夹角，用θ表示，当Y_b轴的正半轴位于过坐标原点的水平面之上时，俯仰角为正，反之为负，旋转范围是-90°～90°。

翻滚角：载体坐标系绕地理坐标系旋转，Z_b轴与通过载体Y_b轴的铅垂面间的夹角，用γ表示。以载体向右倾斜为正，反之为负，旋转范围是-180°～180°。

偏航角：载体坐标系绕地理坐标系旋转，Y_b轴在O_nX_nY_n水平面上的投影与Y_n之间的夹角，用

表示。以载体右偏为正，反之为负，旋转范围是0°～360°。

对应的坐标变换矩阵如下：

偏航角

的旋转矩阵为：

俯仰角θ的旋转矩阵为：

翻滚角γ的旋转矩阵为：

三个基本旋转矩阵进行合并，求出载体坐标系与地理坐标系的旋转矩阵：

式中，

为单位正交矩阵，有：

载体坐标系与地理坐标系之间的转换可以借助姿态矩阵

完成，该姿态矩阵可以简化表示为：

由于地理坐标系到载体坐标系的旋转过程中坐标系始终保持直角坐标系，所以

为正交矩阵，则有：

对比式(6)与式(7)中的姿态角和姿态矩阵，可在已知姿态矩阵的情况下反推求出姿态角，则有：

步骤S200中，掘进机行进过程中，工业相机的视野范围内至少覆盖两个目标。

步骤S200具体步骤如下，控制主机接收t₀时刻惯性系统采集的地理坐标系下X轴、Y轴和Z轴角度参数，并解算出此时掘进机的偏航角

翻滚角γ₀和俯仰角θ₀；控制主机对t₀时刻采集的图像进行处理，采用霍夫变换方法检测图像中每个目标特征的直线，并求出直线的交点，因为本专利所选巷道顶板特征为顶板托盘，呈现正方形，所以每个目标可以求出4个交点，利用PnP算法根据每个目标4个交点的像素坐标求出目标此时的旋转向量rec₀＝[r₁₁,r₁₂,r₁₃]^T和平移向量T₀＝[t₁₁,t₁₂,t₁₃]^T；掘进机行进过程中，求出t₁时刻掘进机的偏航角

翻滚角γ₁和俯仰角θ₁；设定相机和惯性系统的数据采集周期为1s，因为掘进机在煤岩巷道一天行进距离为30～40米，岩巷一天行进距离为3～5米，所以假设在周期t₀和t₁时间段内其平移向量变化量为零。在t₀和t₁时间段内掘进机偏航角变化量为

翻滚角变化量为Δγ＝γ₁-γ₀，俯仰角变化量为Δθ＝θ₁-θ₀；巷道顶板特征是固定不变的，其位姿变化量是相对相机的变化量，相机固定在掘进顶部，因此，掘进机的旋转、平移变化量即为巷道顶板特征的旋转、平移变化量。将t₀时刻的图像中目标的旋转向量rec₀转换为偏航角度

翻滚角γ′₀和俯仰角θ′₀,则t₁时刻图像中目标的偏航角度为

翻滚角γ′₁＝γ′₀+Δγ和俯仰角θ′₁＝θ′₀+Δθ,将t₁时刻图像中目标的旋转角度转换为旋转向量R＝[R₁₁,R₁₂,R₁₃]，根据t₁时刻图像中目标旋转向量R和t₀刻的平移向量T₀利用PnP算法求出t₁时刻图像中每个目标的估计位置；控制主机处理t₁时刻采集的目标图像，采用霍夫变换方法检测图像中每个目标的直线，并求出直线交点的像素坐标，即目标t₁时刻的实际位置。利用中心点匹配算法找到与目标估计位置最近邻的实际位置，即t₀时刻的目标在t₁时刻图像中的位置；根据t₁时刻图像中每个目标的四个交点坐标，采用PnP算法求出每个目标的旋转向量rec₁＝[r′₁₁,r′₁₂,r′₁₃]^T和平移向量T₁＝[t₁₁,t′₁₂,t′₁₃]^T，则t₀和t₁时间段内掘进机的平移向量的增量为相匹配的目标平移向量相减，即ΔT＝T₁-T₀。

步骤S200具体步骤如下，利用PnP算法求出t₁时刻图像中两个目标特征的估计位置，每个目标的像素坐标分别为((x′₁₁,y′₁₁),(x′₁₂,y′₁₂),(x′₁₃,y′₁₃),(x′₁₄,y′₁₄))和((x′₂₁,y′₂₁),(x′₂₂,y′₂₂),(x′₂₃,y′₂₃),(x′₂₄,y′₂₄))，两个目标的中心点估计坐标分别为(x1′_center,y1′_center)和(x2′_center,y2′_center),其中，

利用PnP算法求出t₁时刻图像中两个目标特征的实际位置，每个目标的像素坐标分别为((x₁₁,y₁₁),(x₁₂,y₁₂),(x₁₃,y₁₃),(x₁₄,y₁₄))和((x₂₁,y₂₁),(x₂₂,y₂₂),(x₂₃,y₂₃),(x₂₄,y₂₄))；两个目标的中心点实际坐标分别为(x1_center,y1_center)和(x2_center,y2_center)，其中，

每个目标估计位置与两个目标实际位置的欧氏距离分别为：

其中，distance₁₁为第一个目标估计位置与第一个目标实际位置的欧式距离，distance₁₂为第一个目标估计位置与第二个目标实际位置的欧式距离，distance₂₁为第二个目标估计位置与第一个目标实际位置的欧式距离，distance₂₂为第二个目标估计位置与第二个目标实际位置的欧式距离；如果distance₁₁小于distance₁₂，且distance₂₁大于distance₂₂，则第一个目标的估计位置与第一个实际位置相匹配，第二个目标的估计位置与第二个实际位置相匹配，如果distance₁₁大于distance₁₂，且distance₂₁小于distance₂₂，则第一个目标的估计位置与第二个其实际位置相匹配，第二个目标的估计位置与第一个实际位置相匹配。

与现有技术相比，本发明设计合理，适用性强，稳定性高，可以实现井下掘进机实时的、高精度的位置姿态检测，降低了井下掘进机工作面的作业人数，提高了井下掘进工作面的自动化、智能化集中程度。

附图说明

图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轴角度求出掘进机的姿态角；建立载体坐标系，坐标系原点为掘进机的质心O_b，X_b轴方向为掘进机横轴正右方，Y_b轴方向为掘进机纵轴正前方，Z_b轴方向正交X_b轴、Y_b轴竖直向上。

姿态角的定义：

俯仰角：载体坐标系绕地理坐标系旋转，Y_b轴与O_nX_nY_n水平面的夹角，用θ表示，当Y_b轴的正半轴位于过坐标原点的水平面之上时，俯仰角为正，反之为负，旋转范围是-90°～90°。

翻滚角：载体坐标系绕地理坐标系旋转，Z_b轴与通过载体Y_b轴的铅垂面间的夹角，用γ表示。以载体向右倾斜为正，反之为负，旋转范围是-180°～180°。

偏航角：载体坐标系绕地理坐标系旋转，Y_b轴在O_nX_nY_n水平面上的投影与Y_n之间的夹角，用

表示。以载体右偏为正，反之为负，旋转范围是0°～360°。

对应的坐标变换矩阵如下：

偏航角

的旋转矩阵为：

俯仰角θ的旋转矩阵为：

翻滚角γ的旋转矩阵为：

三个基本旋转矩阵进行合并，求出载体坐标系与地理坐标系的旋转矩阵

式中，

为单位正交矩阵，有：

载体坐标系与地理坐标系之间的转换可以借助姿态矩阵

完成，该姿态矩阵可以简化表示为：

由于地理坐标系到载体坐标系的旋转过程中坐标系始终保持直角坐标系，所以

为正交矩阵，则有：

对比式(6)与式(7)中的姿态角和姿态矩阵，可在已知姿态矩阵的情况下反推求出姿态角，则有：

步骤S200中，掘进机行进过程中，工业相机的视野范围内至少覆盖两个目标。

步骤S200具体步骤如下，控制主机接收t₀时刻惯性系统采集的地理坐标系下X轴、Y轴和Z轴角度参数，并解算出此时掘进机的偏航角

翻滚角γ₀和俯仰角θ₀；控制主机对t₀时刻采集的图像进行处理，采用霍夫变换方法检测图像中每个目标特征的直线，并求出直线的交点，因为本专利所选巷道顶板特征为顶板托盘，呈现正方形，所以每个目标可以求出4个交点，利用PnP算法根据每个目标4个交点的像素坐标求出目标此时的旋转向量rec₀＝[r₁₁,r₁₂,r₁₃]^T和平移向量T₀＝[t₁₁,t₁₂,t₁₃]^T；掘进机行进过程中，求出t₁时刻掘进机的偏航角

翻滚角γ₁和俯仰角θ₁；设定相机和惯性系统的数据采集周期为1s，因为掘进机在煤岩巷道一天行进距离为30～40米，岩巷一天行进距离为3～5米，所以假设在周期t₀和t₁时间段内其平移向量变化量为零。在t₀和t₁时间段内掘进机偏航角变化量为

翻滚角变化量为Δγ＝γ₁-γ₀，俯仰角变化量为Δθ＝θ₁-θ₀；巷道顶板特征是固定不变的，其位姿变化量是相对相机的变化量，相机固定在掘进顶部，因此，掘进机的旋转、平移变化量即为巷道顶板特征的旋转、平移变化量。将t₀时刻的图像中目标的旋转向量rec₀转换为偏航角度

翻滚角γ′₀和俯仰角θ′₀,则t₁时刻图像中目标的偏航角度为

翻滚角γ′₁＝γ′₀+Δγ和俯仰角θ′₁＝θ′₀+Δθ,将t₁时刻图像中目标的旋转角度转换为旋转向量R＝[R₁₁,R₁₂,R₁₃]，根据t₁时刻图像中目标旋转向量R和t₀刻的平移向量T₀利用PnP算法求出t₁时刻图像中每个目标的估计位置；控制主机处理t₁时刻采集的目标图像，采用霍夫变换方法检测图像中每个目标的直线，并求出直线交点的像素坐标，即目标t₁时刻的实际位置。利用中心点匹配算法找到与目标估计位置最近邻的实际位置，即t₀时刻的目标在t₁时刻图像中的位置；根据t₁时刻图像中每个目标的四个交点坐标，采用PnP算法求出每个目标的旋转向量rec₁＝[r′₁₁,r′₁₂,r′₁₃]^T和平移向量T₁＝[t₁₁,t′₁₂,t′₁₃]^T，则t₀和t₁时间段内掘进机的平移向量的增量为相匹配的目标平移向量相减，即ΔT＝T₁-T₀。

步骤S200具体步骤如下，利用PnP算法求出t₁时刻图像中两个目标特征的估计位置，每个目标的像素坐标分别为((x′₁₁,y′₁₁),(x′₁₂,y′₁₂),(x′₁₃,y′₁₃),(x′₁₄,y′₁₄))和((x′₂₁,y′₂₁),(x′₂₂,y′₂₂),(x′₂₃,y′₂₃),(x′₂₄,y′₂₄))，两个目标的中心点估计坐标分别为(x1′_center,y1′_center)和(x2′_center,y2′_center),其中，

利用PnP算法求出t₁时刻图像中两个目标特征的实际位置，每个目标特征的像素坐标分别为((x₁₁,y₁₁),(x₁₂,y₁₂),(x₁₃,y₁₃),(x₁₄,y₁₄))和((x₂₁,y₂₁),(x₂₂,y₂₂),(x₂₃,y₂₃),(x₂₄,y₂₄))；两个目标的中心点实际坐标分别为(x1_center,y1_center)和(x2_center,y2_center)，其中，

每个目标估计位置与两个目标实际位置的欧氏距离分别为

其中，distance₁₁为第一个目标估计位置与第一个目标实际位置的欧式距离，distance₁₂为第一个目标估计位置与第二个目标实际位置的欧式距离，distance₂₁为第二个目标估计位置与第一个目标实际位置的欧式距离，distance₂₂为第二个目标估计位置与第二个目标实际位置的欧式距离；如果distance₁₁小于distance₁₂，且distance₂₁大于distance₂₂，则第一个目标的估计位置与第一个实际位置相匹配，第二个目标的估计位置与第二个实际位置相匹配，如果distance₁₁大于distance₁₂，且distance₂₁小于distance₂₂，则第一个目标的估计位置与第二个其实际位置相匹配，第二个目标的估计位置与第一个实际位置相匹配。

Claims (5)

1.一种基于机器视觉和惯性系统的掘进机位姿检测方法，其特征在于：包括以下步骤，

S100～惯性系统和工业相机刚性连接，并且固定在掘进机机身顶部；惯性系统用来检测掘进机的姿态，根据当地的经纬高参数对惯性系统进行初对准，对准结束后开始采集掘进机地理坐标系下的X轴、Y轴和Z轴角度参数，根据掘进机姿态角的定义，求出其姿态角，并传输至交换机；

S200～工业相机以视频流方式实时显示巷道顶板信息，控制主机以图片格式保存视频流中的帧序列，对图像进行处理，求出图像中目标的位置和旋转、平移参数，其中，巷道顶板特征是巷道的顶板托盘，呈现正方形；

S300～交换机通过以太网与控制主机进行数据传输，控制主机对图像数据进行处理、目标检测与识别、目标跟踪，对惯性系统采集的数据进行数据转换，求出掘进机实时姿态参数；

S400～利用组合导航检测掘进机的位置参数增量与实时姿态参数。

2.根据权利要求1所述的机器视觉和惯性系统的掘进机位姿检测方法，其特征在于：步骤S100中，需要根据掘进机地理坐标系下的X轴、Y轴和Z轴角度求出掘进机的姿态角；建立载体坐标系，坐标系原点为掘进机的质心O_b，X_b轴方向为掘进机横轴正右方，Y_b轴方向为掘进机纵轴正前方，Z_b轴方向正交X_b轴、Y_b轴竖直向上。

姿态角的定义：

俯仰角：载体坐标系绕地理坐标系旋转，Y_b轴与O_nX_nY_n水平面的夹角，用θ表示，当Y_b轴的正半轴位于过坐标原点的水平面之上时，俯仰角为正，反之为负，旋转范围是-90°～90°；

翻滚角：载体坐标系绕地理坐标系旋转，Z_b轴与通过载体Y_b轴的铅垂面间的夹角，用γ表示。以载体向右倾斜为正，反之为负，旋转范围是-180°～180°；

偏航角：载体坐标系绕地理坐标系旋转，Y_b轴在O_nX_nY_n水平面上的投影与Y_n之间的夹角，用

表示。以载体右偏为正，反之为负，旋转范围是0°～360°；

对应的坐标变换矩阵如下：

偏航角

的旋转矩阵为：

俯仰角θ的旋转矩阵为：

翻滚角γ的旋转矩阵为：

三个基本旋转矩阵进行合并，求出载体坐标系与地理坐标系的旋转矩阵：

式中，

为单位正交矩阵，有：

载体坐标系与地理坐标系之间的转换可以借助姿态矩阵

完成，该姿态矩阵可以简化表示为：

由于地理坐标系到载体坐标系的旋转过程中坐标系始终保持直角坐标系，所以

为正交矩阵，则有：

对比式(6)与式(7)中的姿态角和姿态矩阵，可在已知姿态矩阵的情况下反推求出姿态角，则有：

3.根据权利要求2所述的机器视觉和惯性系统的掘进机位姿检测方法，其特征在于：所述的步骤S200中，掘进机行进过程中，工业相机的视野范围内至少覆盖两个目标。

4.根据权利要求3所述的机器视觉和惯性系统的掘进机位姿检测方法，其特征在于：所述的步骤S200具体步骤如下，控制主机接收t₀时刻惯性系统采集的地理坐标系下X轴、Y轴和Z轴角度参数，并解算出此时掘进机的偏航角

翻滚角γ₀和俯仰角θ₀；控制主机对t₀时刻采集的图像进行处理，采用霍夫变换方法检测图像中每个目标特征的直线，并求出直线的交点，因为本专利所选巷道顶板特征为顶板托盘，呈现正方形，所以每个目标可以求出4个交点，利用PnP算法根据每个目标4个交点的像素坐标求出目标此时的旋转向量rec₀＝[r₁₁,r₁₂,r₁₃]^T和平移向量T₀＝[t₁₁,t₁₂,t₁₃]^T；掘进机行进过程中，求出t₁时刻掘进机的偏航角

翻滚角γ₁和俯仰角θ₁；设定相机和惯性系统的数据采集周期为1s，因为掘进机在煤岩巷道一天行进距离为30～40米，岩巷一天行进距离为3～5米，所以假设在周期t₀和t₁时间段内其平移向量变化量为零。在t₀和t₁时间段内掘进机偏航角变化量为

翻滚角变化量为Δγ＝γ₁-γ₀，俯仰角变化量为Δθ＝θ₁-θ₀；巷道顶板特征是固定不变的，其位姿变化量是相对相机的变化量，相机固定在掘进顶部，因此，掘进机的旋转、平移变化量即为巷道顶板特征的旋转、平移变化量。将t₀时刻的图像中目标的旋转向量rec₀转换为偏航角度

翻滚角γ′₀和俯仰角θ′₀,则t₁时刻图像中目标的偏航角度为

翻滚角γ′₁＝γ′₀+Δγ和俯仰角θ′₁＝θ′₀+Δθ,将t₁时刻图像中目标的旋转角度转换为旋转向量R＝[R₁₁,R₁₂,R₁₃]，根据t₁时刻图像中目标旋转向量R和t₀刻的平移向量T₀利用PnP算法求出t₁时刻图像中每个目标的估计位置；控制主机处理t₁时刻采集的目标图像，采用霍夫变换方法检测图像中每个目标的直线，并求出直线交点的像素坐标，即目标t₁时刻的实际位置。利用中心点匹配算法找到与目标估计位置最近邻的实际位置，即t₀时刻的目标在t₁时刻图像中的位置；根据t₁时刻图像中每个目标的四个交点坐标，采用PnP算法求出每个目标的旋转向量rec₁＝[r′₁₁,r′₁₂,r′₁₃]^T和平移向量T₁＝[t₁₁,t′₁₂,t′₁₃]^T，则t₀和t₁时间段内掘进机的平移向量的增量为相匹配的目标平移向量相减，即ΔT＝T₁-T₀。

5.根据权利要求4所述的机器视觉和惯性系统的掘进机位姿检测方法，其特征在于：所述的步骤S400具体步骤如下，利用PnP算法求出t₁时刻图像中两个目标特征的估计位置，每个目标的像素坐标分别为((x′₁₁,y′₁₁),(x′₁₂,y′₁₂),(x′₁₃,y′₁₃),(x′₁₄,y′₁₄))和((x′₂₁,y′₂₁),(x′₂₂,y′₂₂),(x′₂₃,y′₂₃),(x′₂₄,y′₂₄))，两个目标的中心点估计坐标分别为(x1′_center,y1′_center)和(x2′_center,y2′_center),其中，

利用PnP算法求出t₁时刻图像中两个目标特征的实际位置，每个目标特征的像素坐标分别为((x₁₁,y₁₁),(x₁₂,y₁₂),(x₁₃,y₁₃),(x₁₄,y₁₄))和((x₂₁,y₂₁),(x₂₂,y₂₂),(x₂₃,y₂₃),(x₂₄,y₂₄))；两个目标的中心点实际坐标分别为(x1_center,y1_center)和(x2_center,y2_center)，其中，

每个目标估计位置与两个目标实际位置的欧氏距离分别为

其中，distance₁₁为第一个目标估计位置与第一个目标实际位置的欧式距离，distance₁₂为第一个目标估计位置与第二个目标实际位置的欧式距离，distance₂₁为第二个目标估计位置与第一个目标实际位置的欧式距离，distance₂₂为第二个目标估计位置与第二个目标实际位置的欧式距离；如果distance₁₁小于distance₁₂，且distance₂₁大于distance₂₂，则第一个目标的估计位置与第一个实际位置相匹配，第二个目标的估计位置与第二个实际位置相匹配，如果distance₁₁大于distance₁₂，且distance₂₁小于distance₂₂，则第一个目标的估计位置与第二个其实际位置相匹配，第二个目标的估计位置与第一个实际位置相匹配。

Images