CN109115173B - 基于直线定位模型的掘进机机身位姿单目视觉测量方法 - Google Patents

Abstract

本发明公开了一种基于直线定位模型的掘进机机身位姿单目视觉测量方法，通过摄像机对激光束图像进行采集，利用Hough变换对采集的激光束进行直线检测，分别得到平行激光束和水平激光束的直线方程；建立基于两点三线的掘进机位姿解算模型，根据水平激光束的直线方程和平行激光束的直线方程，得到摄像机在定位模型坐标系下的位姿信息，结合掘进机机身坐标系与摄像机坐标系间的转换关系，得到掘进机机身在定位模型坐标下的位姿信息；最后采用全站仪测量激光指向仪绝对坐标，获得激光指向仪在巷道坐标系下坐标，从而得到定位模型坐标系与巷道坐标系的转换关系，结合掘进机机身在定位模型坐标下的位姿信息，最终得到掘进机机身在巷道坐标下的俯仰角、偏航角、翻滚角和位置信息。

Description

基于直线定位模型的掘进机机身位姿单目视觉测量方法

技术领域

本发明属于矿用设备运行状态监测领域，尤其涉及一种基于直线定位模型的掘进机机身位姿单目视觉测量方法。

背景技术

掘进机机身位姿是掘进机工况重要参数，因此掘进机机身位姿检测非常必要。目前煤矿井下掘进机机身位姿测量方法有：基于全站仪的位姿测量系统、基于惯性传感器的位姿测量系统，以及基于机器视觉的位姿测量系统三种。煤矿井下测量环境恶劣，视线易被遮挡，导致基于全站仪的测量系统测量结果不准确。基于惯性传感器的测量系统受设备振动以及惯性传感器本身的累积误差等因素的影响。与现有的矿用设备位姿检测方法相比，视觉定位能够利用特征标靶、机载稳像等方法更好的解决煤矿井下掘进机机身测量问题。对于处理煤矿井下掘进机机身的定位有较大优势。因此，利用机器视觉实现掘进机机身位姿测量是获取掘进机工况参数的一种新方法。

目前,采用机器视觉技术实现掘进机机身位姿测量,国内尚属先例，因此研究实际工况中基于机器视觉技术的掘进机机身位姿测量具有非常重要的实用价值。

发明内容

本发明的目的是提供了一种基于直线定位模型的掘进机机身位姿单目视觉测量方法，能够自动测量出掘进机机身位姿，为煤矿井下掘进机机身位姿测量提供必要的技术支持。

本发明的技术方案是：一种基于直线定位模型的掘进机机身位姿单目视觉测量方法，其特征在于，包括以下步骤：

步骤一、在掘进工作面的巷道顶部安置轨道，轨道上安装两个可沿垂直于掘进方向移动的激光指向仪，保证两个激光指向仪发出两个平行激光束，轨道上再另外安装一个激光指向仪，发出的水平激光束垂直于上述平行激光束，通过固定安装在掘进机上的摄像机对两个平行激光束和一个水平激光束图像进行采集；

步骤二、采用适用于激光束图像的Retinex多尺度增强预处理算法消除步骤一采集到的激光束图像噪声，增强激光束和背景的对比度；

步骤三、对步骤二去噪后的激光束图像进行聚类分割；

步骤四、利用Canny边缘检测方法对步骤三激光束图像聚类分割结果进行处理；

步骤五、利用Hough变换对步骤四得到的激光束图像进行直线检测，分别得到平行激光束和水平激光束的直线方程；

步骤六、利用步骤五提取的水平激光束和平行激光束的直线方程，建立基于两点三线的掘进机位姿解算模型，得到摄像机在定位模型坐标系下的俯仰角α、偏航角β和翻滚角γ和位置信息，结合掘进机机身坐标系与摄像机坐标系间的转换关系，得到掘进机机身在定位模型坐标下的俯仰角、偏航角、翻滚角和位置信息；

步骤七、最后采用全站仪测量激光指向仪绝对坐标，获得激光指向仪在巷道坐标系下坐标，从而得到定位模型坐标系与巷道坐标系的转换关系，根据步骤六得到的掘进机机身在定位模型坐标下的俯仰角、偏航角、翻滚角和位置信息，从而得到掘进机机身在巷道坐标下的俯仰角、偏航角、翻滚角和位置信息。

本发明的有益效果是,本发明融合机器视觉技术、视觉测量技术和图像处理技术，能够根据工作面激光束图像自动计算出掘进机机身的翻滚角、俯仰角、偏航角和位置信息，为巷道掘进精确定向和定位提供了技术支持，具有显著的经济效益和较高的工程应用价值，对实现少人或无人自动掘进控制具有重要意义。

附图说明

图1是掘进工作面激光指向仪安装示意图。

图2是基于两点三线的掘进机位姿解算模型。

图3是本发明掘进机机身位姿测量的流程图。

具体实施方式

下面结合附图，对优选实施例作详细说明，应该强调的是下述说明仅仅是示例性的,而不是为了限制本发明的范围及其应用。

如图3所示，一种基于直线定位模型的掘进机机身位姿单目视觉测量方法，包括以下步骤：

步骤一、如图1所示，在掘进工作面的巷道顶部安置轨道，轨道上安装两个可沿垂直于掘进方向移动的激光指向仪，保证两个激光指向仪发出两个平行激光束，轨道上再另外安装一个激光指向仪，发出的水平激光束垂直于上述平行激光束，通过固定安装在掘进机上的摄像机对两个平行激光束和一个水平激光束图像进行采集；

步骤二、采用适用于激光束图像的Retinex多尺度增强预处理算法，可以有效的消除激光束图像噪声，增强激光束和背景的对比度。

步骤三、对去噪后的激光束图像进行聚类分割；

其中聚类分割的具体步骤如下：

步骤(1)、从激光束图像像素数据样本中选取K个点作为聚类中心；

步骤(2)、计算各个样本到聚类的距离，把样本归到离其最近的聚类中心所在的类；

步骤(3)、计算新形成的每个聚类的数据对象的平均值来得到新的聚类中心；

步骤(4)、反复执行步骤(1)～步骤(4)，直到相邻两次的聚类中心没有任何变化，激光束图像聚类分割结束；

步骤四、利用Canny边缘检测对步骤三掘进工作面激光束图像聚类分割结果进行处理；

步骤五、在对激光束图像分割的基础上，利用Hough变换对分割得到的激光束图像进行直线检测，分别得到平行激光束和水平激光束的直线方程。

步骤六、利用步骤五提取的水平激光束的直线方程和平行激光束的直线方程，建立基于两点三线的掘进机位姿解算模型。

如图2所示，建立基于两点三线的掘进机位姿解算模型的具体过程如下：

步骤(1)建立巷道坐标系O_nX_nY_nZ_n和摄像机坐标系O_cX_cY_cZ_c；L₁,L₃为平行激光束，L₂为水平激光束，假设L_i(i＝1,2,3)的单位方向向量为 V_i(A_i,B_i,C_i)，水平激光束和平行激光束的交点为P₁，P₂，建立以激光束两交点中心为原点的定位模型坐标系OXYZ,P₁在图像上的投影为 p₁(x₁,y₁,z₁)，P₂在图像上的投影为p₂(x₂,y₂,z₂)，激光束L_i在图像平面的投影为图像直线l_i，激光束在图像上投影直线方程设为a_ix+b_iy+c_i =0 ， 直线上任意一点图像坐标为t_i(x_i,y_i,f)，图像直线的方向向量为v_i(-b_i,a_i,0)，设k₁，k₂分别为P₁，P₂与摄像机光心的距离。

步骤(2)设激光束直线、图像投影直线和摄像机光心构成投影平面Si，投影平面S_i的法向量N_i＝(N_i1,N_i2,N_i3)，根据上述条件，得到

步骤(3)激光束直线L₁和激光束直线L₃平行，激光束直线L₃和投影平面S₃的法向量N₃垂直，激光束直线L₁和投影平面S₁的法向量N₁垂直，因此，激光束直线L₁与投影平面S₁的法向量，以及投影平面S₃的法向量N₃都垂直，因此

其中，

A₁＝b₁fc₃-b₃fc₁

B₁＝-a₁fc₃+a₃fc₁

C₁＝a₁b₃f²-a₃b₁f²

同理，得到

另外，激光束直线L₂可以表示为

激光束直线L₂与激光束直线L₁垂直，且激光束直线L₂与投影平面S₂的法向量N₂垂直，因此，激光束直线L₂还可以表示为

其中，

A₂＝a₁b₂c₃f³-a₃b₁b₂f³+a₁c₂c₃f-a₃c₁c₂f

B₂＝-a₁a₂b₃f³+a₂a₃b₁f³+b₁b₂c₃f²-b₂b₃c₁f²

C₂＝-a₁a₂c₃f²+a₂a₃c₁f²-b₁b₂c₃f²+b₂b₃c₁f²

式(4)和式(5)相等，可得

另外根据先验条件，两条平行激光束之间的距离为a，得到

根据式(6)和式(7)，联立解得k₁，k₂，从而计算得到P₁，P₂的空间三维坐标

根据定位模型坐标系下激光束的方向向量V′以及定位模型坐标系下激光束两交点的中心坐标P′，结合激光束两交点的中心在摄像机坐标系下的坐标P，以及根据式(2)、式(3)和式(5)获得的激光束在摄像机坐标系下的空间方向向量V＝(L₁L₂L₃)，根据式(8) 和式(9)得到摄像机坐标系到定位模型坐标系的旋转矩阵R和平移矩阵T

V＝RV′ (8)

P＝RP′+T (9)

其中,

根据上述得到的旋转矩阵R和平移矩阵T，得到摄像机在定位模型坐标系下的俯仰角α、偏航角β和翻滚角γ和位置信息，结合掘进机机身坐标系与摄像机坐标系间的转换关系，得到掘进机机身在定位模型坐标下的俯仰角、偏航角、翻滚角和位置信息。

步骤七、最后采用全站仪测量激光指向仪绝对坐标，获得激光指向仪在巷道坐标系下坐标，从而得到定位模型坐标系与巷道坐标系的转换关系，根据步骤六得到的掘进机机身在定位模型坐标下的俯仰角、偏航角、翻滚角和位置信息，从而得到掘进机机身在巷道坐标下的俯仰角、偏航角、翻滚角和位置信息。

本方法能够自动检测出掘进机的机身位姿，为煤矿井下掘进机机身位姿测量提供必要的技术支持，对于保障掘进机安全运行有着重要的意义。

以上所述仅为本发明较佳的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到的变化或替换，都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应该以权利要求的保护范围为准。

实施例

本实施例的一种基于直线定位模型的掘进机机身位姿单目视觉测量方法，包括：在掘进工作面的巷道顶部安置轨道，轨道上安装两个可沿垂直于掘进方向移动的激光指向仪，保证两个激光指向仪发出两个平行激光束，轨道上再另外安装一个激光指向仪，发出的水平激光束垂直于上述平行激光束，通过固定安装在掘进机上的摄像机对两个平行激光束和一个水平激光束图像进行采集。得到掘进机在定位模型坐标下的实际位姿为：机身的俯仰角、偏航角和翻滚角分别为 28.00°、0.20°和4.50°；机身在X、Y、Z三个方向上的位置分别为-235.00mm、-14.00mm和338.00mm

对采集得到的目标图像采用Retinex多尺度增强预处理；利用 Canny边缘检测对得到的激光束图像聚类分割进行处理；利用Hough 变换对处理得到的激光束图像进行直线检测，得到两个平行激光束和水平激光束的直线方程分别为

L₁激光束直线方程：

x-68y+29088＝0

L₂激光束直线方程：

x-61y+26080＝0

L₃激光束直线方程：

6x-362y+154824＝0

根据上述直线方程，得到三条激光束的两个交点为

p₁(-1.9029,-0.1886,5.0000)p₂(-5.4975,-0.2475,5.0000)

得到的投影平面S_i(i＝1,2,3)的法向量分别为

得到三条激光束摄像机坐标系下的方向向量为

得到P₁，P₂与摄像机光心的距离分别为

k₁＝406.0430k₂＝441.8496

得到P₁，P₂的在摄像机坐标系下的坐标分别为

P₁＝(-144.3335,-14.3033,379.2548)，

P₂＝(-326.6938,-14.7079,297.1294)

根据激光束在摄像机坐标系下的方向向量

以及激光束在定位模型坐标系下的方向向量

根据

V＝RV′

解得

结合掘进机机身坐标系与摄像机坐标系间的转换关系

得到掘进机机身在定位模型坐标下的俯仰角α＝28.2028°、偏航角β＝0.2210°和翻滚角γ＝4.9175°。

根据定位模型坐标系下激光束两交点的中心坐标P′，

P′＝(0 0 0)

结合激光束两交点的中心在摄像机坐标系下的坐标P

P＝(-235.5137,-14.5056,338.1921)

得到

得到定位模型坐标系下掘进机机身X、Y、Z三个方向上的位置分别为-235.5137mm、-14.5056mm和338.1921mm。上述掘进机机身视觉测量的结果误差在允许范围内，可以满足现场使用要求。

煤矿井下实际应用时，需要采用全站仪测量激光指向仪绝对坐标，获得定位模型坐标系与巷道坐标系的转换关系[α′,β′,γ′,x′,y′,z′]，结合上述得到的掘进机机身在定位模型坐标下的俯仰角α、偏航角β、翻滚角γ和位置信息，最终得到掘进机机身在巷道坐标下的俯仰角、偏航角、翻滚角和位置信息。

Claims (1)

1.一种基于直线定位模型的掘进机机身位姿单目视觉测量方法，其特征在于包括以下步骤：

步骤一、在掘进工作面的巷道顶部安置轨道，轨道上安装两个可沿垂直于掘进方向移动的激光指向仪，保证两个激光指向仪发出两个平行激光束，轨道上再另外安装一个激光指向仪，发出的水平激光束垂直于上述平行激光束，通过固定安装在掘进机上的摄像机对两个平行激光束和一个水平激光束图像进行采集；

步骤二、采用适用于激光束图像的Retinex多尺度增强预处理算法消除步骤一采集到的激光束图像噪声，增强激光束和背景的对比度；

步骤三、对步骤二去噪后的激光束图像进行聚类分割；

步骤四、利用Canny边缘检测方法对步骤三激光束图像聚类分割结果进行处理；

步骤五、利用Hough变换对步骤四得到的激光束图像进行直线检测，分别得到平行激光束和水平激光束的直线方程；

步骤六、利用步骤五提取的水平激光束和平行激光束的直线方程，建立基于两点三线的掘进机位姿解算模型，得到摄像机在定位模型坐标系下的俯仰角α、偏航角β和翻滚角γ和位置信息，结合掘进机机身坐标系与摄像机坐标系间的转换关系，得到掘进机机身在定位模型坐标下的俯仰角、偏航角、翻滚角和位置信息；

步骤七、最后采用全站仪测量激光指向仪绝对坐标，获得激光指向仪在巷道坐标系下坐标，从而得到定位模型坐标系与巷道坐标系的转换关系，根据步骤六得到的掘进机机身在定位模型坐标下的俯仰角、偏航角、翻滚角和位置信息，从而得到掘进机机身在巷道坐标下的俯仰角、偏航角、翻滚角和位置信息；

其中步骤六建立基于两点三线的掘进机位姿解算模型的具体过程如下：

步骤(1)、建立巷道坐标系O_nX_nY_nZ_n和摄像机坐标系O_cX_cY_cZ_c；L₁,L₃为平行激光束，L₂为水平激光束，假设L_i(i＝1,2,3)的单位方向向量为V_i(A_i,B_i,C_i)，水平激光束和平行激光束的交点为P₁，P₂，建立以激光束两交点中心为原点的定位模型坐标系OXYZ,P₁在图像上的投影为p₁(x₁,y₁,z₁)，P₂在图像上的投影为p₂(x₂,y₂,z₂)，激光束L_i在图像平面的投影为图像直线l_i，激光束在图像上投影直线方程设为a_ix +b_iy+c_i =0 ， 直线上任意一点图像坐标为t_i(x_i,y_i,f)，图像直线的方向向量为v_i(-b_i,a_i,0)，设k₁，k₂分别为P₁，P₂与摄像机光心的距离；

步骤(2)、设激光束直线、图像投影直线和摄像机光心构成投影平面S_i，投影平面S_i的法向量N_i＝(N_i1,N_i2,N_i3)，根据上述条件，得到

步骤(3)、激光束直线L₁和激光束直线L₃平行，激光束直线L₃和投影平面S₃的法向量N₃垂直，激光束直线L₁和投影平面S₁的法向量N₁垂直，因此，激光束直线L₁与投影平面S₁的法向量，以及投影平面S₃的法向量N₃都垂直，因此

其中，

A₁＝b₁fc₃-b₃fc₁

B₁＝-a₁fc₃+a₃fc₁

C₁＝a₁b₃f²-a₃b₁f²

同理，得到

另外，激光束直线L₂可以表示为

激光束直线L₂与激光束直线L₁垂直，且激光束直线L₂与投影平面S₂的法向量N₂垂直，因此，激光束直线L₂还可以表示为

其中，

A₂＝a₁b₂c₃f³-a₃b₁b₂f³+a₁c₂c₃f-a₃c₁c₂f

B₂＝-a₁a₂b₃f³+a₂a₃b₁f³+b₁b₂c₃f²-b₂b₃c₁f²

C₂＝-a₁a₂c₃f²+a₂a₃c₁f²-b₁b₂c₃f²+b₂b₃c₁f²

式(4)和式(5)相等，可得

另外根据先验条件，两条平行激光束之间的距离为_a，得到

根据式(6)和式(7)，联立解得k₁，k₂，从而计算得到P₁，P₂的空间三维坐标

根据定位模型坐标系下激光束的方向向量V′以及定位模型坐标系下激光束两交点的中心坐标P′，结合激光束两交点的中心在摄像机坐标系下的坐标P，以及根据式(2)、式(3)和式(5)获得的激光束在摄像机坐标系下的空间方向向量V＝(L₁ L₂ L₃)，根据式(8)和式(9)得到摄像机坐标系到定位模型坐标系的旋转矩阵R和平移矩阵T

V＝RV′ (8)

P＝RP′+T (9)

其中，

根据上述得到的旋转矩阵R和平移矩阵T，得到摄像机在定位模型坐标系下的俯仰角α、偏航角β和翻滚角γ和位置信息，结合掘进机机身坐标系与摄像机坐标系间的转换关系，得到掘进机机身在定位模型坐标下的俯仰角、偏航角、翻滚角和位置信息。

Images