CN110108202B - 一种液压支架位姿检测装置和方法 - Google Patents

Abstract

本发明公开了一种液压支架位姿检测装置和方法，仅利用液压支架底座上特征轮廓的三个点就可以对液压支架的六个自由度位姿进行检测，方便、快捷、简单、精度较高。该方法也可对其它领域的物体位姿检测提供借鉴。

Description

一种液压支架位姿检测装置和方法

技术领域

本发明涉及智能开采技术领域，特别是涉及一种利用激光雷达对液压支架位姿进行检测的装置和方法。

背景技术

煤矿开采的自动化、智能化和无人化已成大势所趋，但工作面液压支架的直线度和位姿检测及控制问题极大地困扰着国内无人工作面的无人化进程。综采工作面中液压支架和刮板输送机互为支点，通过电液控制系统控制推拉油缸的伸缩，实现对二者的相互推拉，交替前进，从而完成工作面连续平稳推进。其中，每个液压支架位姿和整体支架群直线度是衡量工作面质量的一个重要指标，只有液压支架整体直线度符合要求，才能保证把刮板输送机推直，从而把煤壁切直，实现工作面的“三直”。因为支架群的直线度是由所有单个液压支架位姿所决定，而单个液压支架空间六自由度位姿除了反映了自身的位姿信息外，还部分反映顶地板的地质信息、其与刮板输送机受力信息及推拉油缸的精度信息等，所以检测液压支架的空间六自由度位姿一直以来都是学者们研究的对象，大家利用各种方法和装置，均取得了一定的成果。

发明内容

本发明的目的是为解决上述现有技术的不足之处而提供一种液压支架位姿检测装置和方法。

为解决上述技术问题，本发明采用的一个技术方案是：提供一种液压支架位姿检测装置，包括：

控制显示部分和扫描执行部分；所述扫描显示部分设置于所述控制显示部分的上部；

其中，所述扫描执行部分包括42步进电机，联轴器，滚珠丝杆固定座，滚珠螺母和滑轨连接器，滚珠螺母，滚珠丝杆，滑轨，滑块，滑轨和激光雷达连接装置，激光雷达，防爆外壳；两个滚珠丝杆固定座安装在防爆外壳上面并支撑着滚珠丝杆，滚珠螺母套在滚珠丝杆上，通过滚珠螺母和滑轨连接器与滑轨的一端固定，滑块固定在防爆外壳上，滑轨穿过滑块后通过滑轨雷达连接器与激光雷达连接；当滚珠丝杆由电机驱动正反转旋转时，滑轨受滑块约束只能沿一条直线运动，从而带动激光雷达前后运动；

控制显示部分包括液晶显示器，第一行程开关，电机驱动，控制器及电源和第二行程开关；在防爆外壳内，通过电源进行供电，控制器由arduino作为控制核心，其通过A4988芯片来控制42步进电机的正反转和速度，通过第一行程开关和第二行程开关控制激光雷达的起始位置和最大位移，通过读取激光雷达对液压支架特征点的角度和距离数据，并结合42步进电机速度和运行时间，经过计算，得到液压支架相对检测装置的六自由度位姿，然后把计算出的位姿信息通过防爆外壳外液晶显示器进行显示输出。

为解决上述技术问题，本发明采用的一个技术方案是：提供一种液压支架位姿检测方法，利用如前述技术方案所述的位姿检测装置进行检测，包括以下步骤：利用机器人携带位姿检测装置行走在液压支架人工行走平台上，当其行走到支架上四个立柱的中心位置时，启动装置对液压支架相对机器人的位姿进行检测；同步启动42步进电机和激光雷达，从t＝0开始计时，设定滑轨的直线运动速度为v，检测三个待测角度点A、B、C的坐标，以求解液压支架相对机器人及检测装置的位姿；

在液压显示器上显示解液压支架相对机器人及检测装置的六个位姿参数值。

其中，求解液压支架相对机器人及检测装置的位姿的步骤包括：

检测出A、B、C点坐标^RA＝(Ax，Ay，Az)，^RB＝(Bx，By，Bz)，^RC＝(Cx，Cy，Cz)；

求解AC直线的方程，再根据过点B的直线BO_S与直线AC垂直条件，求出垂足O_S的坐标，^RO_s＝(Osx，Osy，Osz)；

设定待测支架相对检测装置的位姿为L(S，R)＝(δ，ε，ζ，d，e，f)，O_S在{R}系的坐标表示为^RO_s＝(Osx，Osy，Osz)，其坐标值即为其在{R}系的位置，即

d＝Osx

e＝Osy

f＝Osz；

根据得到的O_S，A，B三点的坐标，{S}系中两坐标轴Y_S，Z_S在{R}系中用向量表示为：

O_SY_S＝O_SA＝(Ax-Osx)i+(Ay-Osy)j+(Az-Osz)k＝(a2i+b2j+c2k)m1

O_SZ_S＝O_SB＝(Bx-Osx)i+(By-Osy)j+(Bz-Osz)k＝(a3i+b3j+c3k)m2

设定{S}坐标系为右手坐标系，则X_S轴在{R}系向量为：

O_SX_S＝O_SA×O_SB＝[(Ay-Osy)(Bz-Osz)-(By-Osy)(Az-Osz)]i+[(Az-Osz)(Bx-Osx)-(Bz-Osz)(Ax-Osx)]j+[(Ax-Osx)(By-Osy)-(Bx-Osx)(Ay-Osy)]k＝(a1i+b1j+c1k)m3

其中，m1，m2，m3为比例系数。a1，b1，c1，a2，b2，c2，a3，b3，c3为O_SX_S，O_SY_S，O_SZ_S三个坐标轴单位向量在X_R，Y_R，Z_R轴的分量；

根据单位向量的值求出{S}系各坐标轴相对{R}系各坐标轴方向余弦:

X_S轴相对于X_R，Y_R，Z_R三轴的余弦为：

Y_S轴相对于X_R，Y_R，Z_R三轴的余弦为：

Z_S轴相对于X_R，Y_R，Z_R三轴的余弦为：

则从{R}系到{S}系的变换矩阵为：

根据RPY角反解公式得：

其中，ζ，ε，δ三个角度值都在[-90°，90°]区间内。

区别于现有技术，本发明的液压支架位姿检测装置和方法利用液压支架底座上特征轮廓的三个点就可以对液压支架的六个自由度位姿进行检测，方便、快捷、简单、精度较高。该方法也可对其它领域的物体位姿检测提供借鉴。

附图说明

图1是本发明提供的一种液压支架位姿检测装置的结构示意图。

图2是本发明提供的一种液压支架位姿检测装置的内部结构控制关系示意图。

图3本发明提供的一种液压支架位姿检测装置的检测作业方法示意图。

图4是本发明提供的一种液压支架位姿检测方法中对特征点A的坐标进行检测的程序控制流程图。

其中，1-42步进电机，2-联轴器，3-滚珠丝杆固定座，4-滚珠螺母和滑轨连接器，5-滚珠螺母，6-滚珠丝杆，7-滑轨，8-滑块，9-滑轨和激光雷达连接装置，10-激光雷达，11-防爆外壳，12-液晶显示器，13-第二行程开关，14-电机驱动，15-控制器及电源，16-第一行程开关，21-液压支架，22-位姿检测装置，23-机器人。

具体实施方式

在下面的描述中阐述了很多具体细节以便于充分理解本发明。但是本发明能够以很多不同于在此描述的其它方式来实施，本领域技术人员可以在不违背本发明内涵的情况下做类似推广，因此本发明不受下面公开的具体实施的限制。

其次，本发明利用示意图进行详细描述，在详述本发明实施例时，为便于说明，示意图只是实例，其在此不应限制本发明保护的范围。

如图1所示，为本发明装置示意图，包括控制显示部分和扫描执行部分；所述扫描显示部分设置于所述控制显示部分的上部；

其中，所述扫描执行部分包括42步进电机1，联轴器2，滚珠丝杆固定座3，滚珠螺母和滑轨连接器4，滚珠螺母5，滚珠丝杆6，滑轨7，滑块8，滑轨和激光雷达连接装置9，激光雷达10，防爆外壳11；两个滚珠丝杆固定座3安装在防爆外壳11上面并支撑着滚珠丝杆6，滚珠螺母5套在滚珠丝杆6上，通过滚珠螺母和滑轨连接器4与滑轨7的一端固定，滑块8固定在防爆外壳上，滑轨7穿过滑块8后通过滑轨雷达连接器9与激光雷达10连接；当滚珠丝杆6由电机驱动正反转旋转时，滑轨7受滑块8约束只能沿一条直线运动，从而带动激光雷达9前后运动；

控制显示部分包括液晶显示器12，第一行程开关13，电机驱动14，控制器及电源15和第二行程开关16；在防爆外壳11内，通过电源15进行供电，控制器由arduino作为控制核心，其通过A4988芯片来控制42步进电机1的正反转和速度，通过第一行程开关13和第二行程开关16控制激光雷达9的起始位置和最大位移，通过读取激光雷达9对液压支架特征点的角度和距离数据，并结合42步进电机1速度和运行时间，经过计算，得到液压支架相对检测装置的六自由度位姿，然后把计算出的位姿信息通过防爆外壳11外液晶显示器12进行显示输出。

如图2所示，为本发明装置的各部件之间的控制关系图，控制器首先通过读取第一行程开关16的信息标定激光雷达9初始位置和时间，然后通过控制电机的旋向和转速，达到间接控制激光雷达9的位置，在激光雷达9扫描过程中，通过读取激光雷达9的旋转角度和特定点的距离值，计算出液压支架位姿信息，最后通过液晶显示器输出。

在位姿检测时，涉及的组件包括待测液压支架21，本发明装置22和携带本发明装置的机器人23。规定待测支架坐标系为{S}，机器人上安装的本发明装置坐标系为{R}，且均为右手坐标系，{S}系相对于{R}系的位姿可以这样来描述：刚开始时两个坐标系完全重合，{S}系经过了一系列各种复杂的运动到达了某一位姿，现在不考虑其具体运动过程，认为其目前任何位姿都可以通过六次有序运动到达，即首先通过绕{R}系的X轴、Y轴、Z轴各旋转了一定角度δ，ε，ζ，再通过沿{R}系的X轴、Y轴、Z轴各平移了一定距离d，e，f到达了现在的位姿，此时就可以用六个有序数列来表示{S}系相对{R}系的位姿：

L(S，R)＝(δ，ε，ζ，d，e，f)

该表示法是用从一个坐标系到另一个坐标系的运动过程来描述两个坐标系的相对位姿，用该方法优点是直观、简单，对两个支架位姿的变换过程一目了然，把运动、变换和位姿很好地联系了起来，对后续在三维虚拟环境下还原各液压支架位姿提供了便利。

特征点就是指液压支架特征轮廓上代表Ys轴的A、C两点和代表Zs轴的B点，共三个点。理论上，在Ys轴上的任意两点都可以代表该轴，但由于测量过程中误差的存在，在选择A、C两点时，应使C点尽量靠近Os点，并使A点距离C点尽可能的远，这样才能最大程度的缩小由于C、A两点测量误差传递给直线AC的误差。同理，应使B点尽量远离Os点。

如图3所示是机器人在支架型号为ZZ4000-18-38上的情况。在理想情况下，机器人坐标系各轴与支架坐标系相应各轴相互平行，且机器人位于两支柱中部，激光雷达旋转平面与平面ABC重合。此时，机器人坐标系原点O_R距离Z_S轴距离约650mm，距离YS轴约515mm。根据前面讨论A、C、B三点的选取原则，结合实际情况，选取O_RA、O_RC、O_RB与激光雷达零位线的夹角分别为φ_A＝-30°，φ_C＝25°，φ_B＝85°时较为理想，此时，L_A＝515/cos(-30°)≈590.05mm，L_C＝515/cos25°≈568.24mm，L_B＝650/cos5°≈652.48mm。对液压支架位姿的检测通常是在非理想时的状态。

如图4所示，为特征点A的坐标检测过程程序流程图，当激光雷达从开始逐渐右移时，首先在雷达第一周的角度-31°＜φ＜-29°内选取一点(因为激光雷达在输出值数据时每一周的角度都会有微小变化，所以无法读取-30°整的值)，读取其值记入L_A，当580.05＜L_A＜600.05mm时认为其有效，否则从第二周开始重新选点。取得有效值后，再把从下一周期内取得的点记入L_A2，并比较两者值大小，如果没有突变，则把L_A2的值赋给L_A。当其扫描离开支架轮廓时，激光测距的值会有一个突变，当突变值大于±20mm时，认为到达了轮廓边缘，读取t的值，记突变前的值L_A为特征点距离值。根据Ax＝-vt，Ay＝-LAsinφ，Az＝-LAcosφ，就确定了特征点A的坐标，A点选择过程结束。其它两点选取过程与之相似。

一种液压支架位姿检测方法，利用前述技术方案所述的位姿检测装置进行检测，包括以下步骤：

利用机器人携带位姿检测装置行走在液压支架人工行走平台上，当其行走到支架上四个立柱的中心位置时，启动装置对液压支架相对机器人的位姿进行检测；

同步启动42步进电机2和激光雷达9，从t＝0开始计时，设定滑轨7的直线运动速度为v，检测三个待测角度点A、B、C的坐标，以求解液压支架相对机器人及检测装置的位姿；

在液压显示器上显示解液压支架相对机器人及检测装置的六个位姿参数值。

同步启动步进电机和激光雷达，从t＝0开始计时，设定滑轨的直线运动速度为v，准备检测三个特定角度点A、B、C的距离。

检测代表Y_S轴的两个特征点A和C的坐标，再检测Z_S轴上一个特征点B的坐标，求出液压支架相对机器人及检测装置位姿。

第一步，检测出A点坐标^RA＝(Ax，Ay，Az)，同理，可得到B、C两点坐标^RB＝(Bx，By，Bz)，^RC＝(Cx，Cy，Cz)；

第二步，求出AC直线的方程，再根据过点B的直线BO_S与直线AC垂直条件，求出垂足O_S的坐标，^RO_s＝(Osx，Osy，Osz)；

第三步，计算待测支架相对检测装置的位姿L(S，R)＝(δ，ε，ζ，d，e，f)。

1)求解{S}系相对{R}系位置

OS的坐标为^RO_s＝(Osx，Osy，Osz)，其坐标值即为其在{R}系的位置，所以

d＝Osx

e＝Osy

f＝Osz

2)求解{S}系相对{R}系姿态

因为已经得到O_S，A，B三点的坐标，所以{S}系中两坐标轴Y_S，Z_S在{R}系中可以用向量表示为：

O_SY_S＝O_SA＝(Ax-Osx)i+(Ay-Osy)j+(Az-Osz)k＝(a2i+b2j+c2k)m1

O_SZ_S＝O_SB＝(Bx-Osx)i+(By-Osy)j+(Bz-Osz)k＝(a3i+b3j+c3k)m2

由于{S}坐标系是右手坐标系，所以X_S轴在{R}系向量为：

O_SX_S＝O_SA×O_SB＝[(Ay-Osy)(Bz-Osz)-(By-Osy)(Az-Osz)]i+[(Az-Osz)(Bx-Osx)-(Bz-Osz)(Ax-Osx)]j+[(Ax-Osx)(By-Osy)-(Bx-Osx)(Ay-Osy)]k＝(a1i+b1j+c1k)m3

其中，m1，m2，m3为比例系数。a1，b1，c1，a2，b2，c2，a3，b3，c3为O_SX_S，O_SY_S，O_SZ_S三个坐标轴单位向量在X_R，Y_R，Z_R轴的分量。

根据单位向量的值可以求出{S}系各坐标轴相对{R}系各坐标轴方向余弦:

X_S轴相对于X_R，Y_R，Z_R三轴的余弦为：

Y_S轴相对于X_R，Y_R，Z_R三轴的余弦为：

Z_S轴相对于X_R，Y_R，Z_R三轴的余弦为：

所以从{R}系到{S}系的变换矩阵为

根据RPY角反解公式得：

其中，ζ，ε，δ三个角度值都在[-90°，90°]区间内。

在液压显示器上显示(δ，ε，ζ，d，e，f)六个位姿参数值。

与现有技术相比，本发明液压支架位姿检测装置和方法仅利用液压支架底座上特征轮廓的三个点就可以对液压支架的六个自由度位姿进行检测，方便、快捷、简单、精度较高。该方法也可对其它领域的物体位姿检测提供借鉴。

本发明虽然已以较佳实施例公开如上，但其并不是用来限定本发明，任何本领域技术人员在不脱离本发明的精神和范围内，都可以利用上述揭示的方法和技术内容对本发明技术方案做出可能的变动和修改，因此，凡是未脱离本发明技术方案的内容，依据本发明的技术实质对以上实施例所作的任何简单修改、等同变化及修饰，均属于本发明技术方案的保护范围。

Claims (3)

1.一种液压支架位姿检测装置，其特征在于，包括控制显示部分和扫描执行部分；所述扫描显示部分设置于所述控制显示部分的上部；

其中，所述扫描执行部分包括42步进电机(1)，联轴器(2)，滚珠丝杆固定座(3)，滚珠螺母和滑轨连接器(4)，滚珠螺母(5)，滚珠丝杆(6)，滑轨(7)，滑块(8)，滑轨和激光雷达连接装置(9)，激光雷达(10)，防爆外壳(11)；两个滚珠丝杆固定座(3)安装在防爆外壳(11)上面并支撑着滚珠丝杆(6)，滚珠螺母(5)套在滚珠丝杆(6)上，通过滚珠螺母和滑轨连接器(4)与滑轨(7)的一端固定，滑块(8)固定在防爆外壳上，滑轨(7)穿过滑块(8)后通过滑轨雷达连接器(9)与激光雷达(10)连接；当滚珠丝杆(6)由电机驱动正反转旋转时，滑轨(7)受滑块(8)约束只能沿一条直线运动，从而带动激光雷达(9)前后运动；

控制显示部分包括液晶显示器(12)，第一行程开关(13)，电机驱动(14)，控制器及电源(15)和第二行程开关(16)；在防爆外壳(11)内，通过电源(15)进行供电，控制器由arduino作为控制核心，其通过A4988芯片来控制42步进电机(1)的正反转和速度，通过第一行程开关(13)和第二行程开关(16)控制激光雷达(9)的起始位置和最大位移，通过读取激光雷达(9)对液压支架三个特征点A、B、C的角度和距离数据，其中，A和C是液压支架坐标系下YS轴上的两点，B为液压支架坐标系下ZS轴上的一点；结合42步进电机(1)速度和运行时间，经过计算，得到液压支架相对检测装置的六自由度位姿，然后把计算出的位姿信息通过防爆外壳(11)外液晶显示器(12)进行显示输出。

2.一种液压支架位姿检测方法，利用如权利要求1所述的位姿检测装置进行检测，其特征在于，包括以下步骤：

利用机器人携带位姿检测装置行走在液压支架人工行走平台上，当其行走到支架上四个立柱的中心位置时，启动装置对液压支架相对机器人的位姿进行检测；

同步启动42步进电机(2)和激光雷达(9)，从t＝0开始计时，设定滑轨(7)的直线运动速度为v，检测三个待测特征点A、B、C的坐标，以求解液压支架相对机器人及检测装置的位姿；

在液压显示器上显示液压支架相对机器人及检测装置的六个位姿参数值。

3.根据权利要求2所述的液压支架位姿检测方法，其特征在于，求解液压支架相对机器人及检测装置的位姿的步骤包括：

检测出3个特征点A、B、C点在液压支架坐标系下的坐标^RA＝(Ax，Ay，Az)，^RB＝(Bx，By，Bz)，^RC＝(Cx，Cy，Cz)；其中，规定待测液压支架坐标系为{R}，位姿检测坐标系为{S}；

求解AC直线的方程，再根据过点B的直线BO_S与直线AC垂直条件，求出垂足O_S的坐标，^RO_S＝(Osx，Osy，Osz)；

设定待测支架相对检测装置的位姿为L(S，R)＝(δ，ε，ζ，d，e，f)，O_S在{R}系的坐标表示为^RO_S＝(Osx，Osy，Osz)，其坐标值即为其在{R}系的位置，即

d＝Osx

e＝Osy

f＝Osz；

根据得到的O_S，A，B三点的坐标，{S}系中两坐标轴Y_S，Z_S在{R}系中用向量表示为：

O_SY_S＝O_SA＝(Ax-Osx)i+(Ay-Osy)j+(Az-Osz)k＝(a2i+b2j+c2k)m1

O_SZ_S＝O_SB＝(Bx-Osx)i+(By-Osy)j+(Bz-Osz)k＝(a3i+b3j+c3k)m2

设定{S}坐标系为右手坐标系，则X_S轴在{R}系向量为：

O_SX_S＝O_SA×O_SB＝[(Ay-Osy)(Bz-Osz)-(By-Osy)(Az-Osz)]i+[(Az-Osz)(Bx-Osx)-(Bz-Osz)(Ax-Osx)]j+[(Ax-Osx)(By-Osy)-(Bx-Osx)(Ay-Osy)]k＝(a1i+b1j+c1k)m3

其中，m1，m2，m3为比例系数；a1，b1，c1，a2，b2，c2，a3，b3，c3为O_SX_S，O_SY_S，O_SZ_S三个坐标轴单位向量在X_R，Y_R，Z_R轴的分量；

根据单位向量的值求出{S}系各坐标轴相对{R}系各坐标轴方向余弦:

X_S轴相对于X_R，Y_R，Z_R三轴的余弦为：

Y_S轴相对于X_R，Y_R，Z_R三轴的余弦为：

Z_S轴相对于X_R，Y_R，Z_R三轴的余弦为：

则从{R}系到{S}系的变换矩阵为：

根据RPY角反解公式得：

其中，ζ，ε，δ三个角度值都在[-90°，90°]区间内。

Images