专利名称：掘进机位姿检测系统和掘进机的制作方法
技术领域：
本发明涉及工程机械领域，具体而言，涉及掘进机位姿检测系统和掘进机。
背景技术：
悬臂式掘进机是一种能够完成截割、装载、转载煤岩，并能自己行走，具有喷雾灭尘等功能的巷道掘进设备，是煤矿井下生产中应用最广泛的巷道掘进机械装备。随着科学技术的进步，要求巷道施工向安全、优质、高效、无人化方向发展，因此，加强掘进机向智能化方向发展是一个大的趋势。然而，目前煤矿井下的掘进机操作手主要是以激光指向仪发射的激光光束为指引方向，通过手动操作控制掘进机前进运动轨迹，随机性比较大。因为煤矿井下工作环境极其恶劣，粉尘浓度大、光线黑暗，严重影响操作手的视线，容易造成欠挖、过挖现象，给井下工人增加了工作量，降低了工作效率，所以针对上述掘进机工作缺点，研发人员不断提出了掘进机的姿态检测方法，但其测量精度不高，准确性差，且结构复杂，可引发的故障点多，整个系统的稳定性差，实际可操作性差。因此，需要一种掘进机位姿检测技术，可准确检测出掘进机的位姿参数，且结构设计简单，系统结构稳定性高。
发明内容
考虑到上述背景技术，本发明所要解决的一个技术问题是提供一种掘进机位姿检测技术，可准确检测出掘进机的位姿参数，且结构设计简单，系统结构稳定性高。根据本发明的一个方面，提供了一种掘进机位姿检测系统，包括激光指向单元、激光幕布式标靶、第一采集单元、第二采集单元和数据采集模块，其中，所述激光指向单元安装在煤矿井下的巷道上方，发射激光束；所述激光幕布式标靶安装在掘进机上，接收所述激光指向单元发射的激光束；所述第一采集单元采集所述激光幕布式标靶所呈现的激光点图像并将激光点图像数据传送至所述数据采集模块；所述第二采集单元安装在所述掘进机本体上，检测所述第二采集单元相对于预定坐标系的角度信息并将所述角度信息发送至所述数据采集模块；所述数据采集模块接收来自所述第一采集单元和所述第二采集单元的检测结果，根据所述检测结果确定所述掘进机的位姿参数。激光指向单元为掘进机确定行走方向的基准，为了避免人工观察激光的指向，可在掘进机上安装激光幕布式标靶，来确定激光相对于掘进机的位置，从而可确定掘进机相对于激光光束的位置，并且还通过第二采集单元来采集自身相对于预定坐标系的角度信息，从而可确定掘进机相对于预定坐标系的角度信，通过上述两个信息便可准确确定掘进机的位姿参数，并且涉及的元器件结构简单，进一步提高了系统的稳定性。在上述技术方案中，优选的，所述数据采集模块包括图像处理单元和数据处理单元，其中，所述图像处理单元连接至所述第一采集单元，根据所述激光点图像数据计算出所述掘进机的水平偏向位移和垂直偏向位移，并将所述水平偏向位置和垂直偏向位移的数据传送至所述数据处理单元；所述数据处理单元接至所述图像处理单元和所述第二采集单元，根据所述图像处理单元的图像处理结果和所述第二采集单元的检测结果得到所述掘进机的位姿参数，所述位姿参数包括偏向角、俯仰角、滚动角、水平偏向位移和垂直偏向位移。根据采集的激光点图像便可以知道掘进机的水平偏向位置和垂直偏向位置，以及根据第二采集单元的检测结果便可以知道掘进机的偏向角、俯仰角、滚动角等角度信息，根据这些参数便能够准确测出掘进机相对于激光方向的位置和姿态。在上述任一技术方案中，优选的，所述数据处理单元还连接至所述掘进机的控制器，用于判断所述掘进机的偏向角是否大于角度警戒值，在大于所述角度警戒值时，将角度调整命令发送至所述控制器，以调整所述掘进机的机身角度。在检测出掘进机的偏向角之后，进而判断该偏向角是否处于设定的角度警戒值，若大于，说明掘进机的姿态已不符合理想姿态，需调整掘进机的机身角度，因此向掘进机的控制发送控制信号，控制信号包含调整的角度范围和方向，掘进机收到该控制信号后，向执行机构发送调节命令，利用掘进机行走电磁阀的开度从而调整掘进机的机身角度。 在上述任一技术方案中，优选的，所述数据处理单元还用于判断所述水平偏向位移是否大于水平位移警戒值，在大于所述水平位移警戒值时，计算出所述掘进机的左履带速度和右履带速度并获取左右履带速度差，将所述左右履带速度差传送至所述控制器，以调整所述掘进机的左履带速度和/或右履带速度。同样的，通过水平位移可确定掘进机偏离航道的程度，当水平大于设定的水平位移警戒值，说明需调整掘进机的位移，使得激光点能打在激光标靶上的理想位置，通过控制器调整左右履带的行驶速度，使掘进机在行进的过程中可往左移或往右移，从而调节掘进机的水平位移。在上述任一技术方案中，优选的，所述数据处理单元还用于在判断出所述水平偏向位移小于等于所述水平位移警戒值且所述偏向角大于所述角度警戒值时，将角度调整指令发送至所述控制器，以调整所述掘进机的机身角度。在判断时还需综合考虑偏向角和水平位移，虽然水平位移小于水平位移警戒值，但若偏向角仍大于角度警戒值，则仍需调整掘进机的机身角度。在上述任一技术方案中，优选的，还可以包括显示器，连接至所述数据采集模块，显示所述掘进机的位姿参数。方便操作人员清楚掘进机的当前位置和姿态并进行相应的操作，当发生危险时，该显示器还可以发出警报提示。在上述任一技术方案中，优选的，还可以包括固定装置，设置在所述掘进机上并对准所述掘进机的中轴线，其中，所述固定装置包括第一侧面和第二侧面，所述第一侧面与所述第二侧面均垂直于所述中轴线，所述激光幕布式标靶安装在所述第一侧面上，所述第一采集单元安装所述第二侧面上并对准所述激光标靶。通过该固定装置将相应的元器件固定在掘进机的机身上，且能够使激光打在激光标靶上，调整激光标靶相对于激光的理想位置，这样，才可通过激光点的位置信息来准确调整掘进机的位移，使掘进机沿着激光方向行走。在上述任一技术方案中，优选的，所述数据采集模块还用于在所述第一采集单元未采集到所述激光点图像或所述激光点图像出现异常时，向所述掘进机的控制器发送停止指令，使所述掘进机停止前进。由于突发情况可能会造成激光点脱靶、突然消失或时断时续，当检测到激光标靶上没有激光点时，向掘进机发送停止命令，掘进机收到该命令后停止前进，防止掘进机偏离航道而碰到巷道壁，进而出现危险。在上述任一技术方案中，优选的，所述第一采集单元包括CXD摄像头，所述第二采集单元包括航姿仪。利用航姿仪测量精度高，结构简单，煤矿井下环境恶劣，利用C⑶摄像头可防止受污染。根据本发明的另一方面，还提供了一种掘进机，包括如上述任一技术方案中所述的掘进机位姿检测系统。
图I示出了根据本发明的实施例的掘进机位姿检测系统的示意图；图2示出了根据本发明的实施例的掘进机位姿检测系统安装于掘进机的示意图；·
图3示出了根据本发明的实施例的掘进机位姿检测的流程图；图4示出了根据本发明的实施例的调整掘进机位姿的流程图；图5示出了根据本发明的实施例的调整掘进机的轨迹的示意图。
具体实施例方式为了能够更清楚地理解本发明的上述目的、特征和优点，下面结合附图和具体实施方式
对本发明进行进一步的详细描述。下面结合附图和实施例对本发明做进一步说明。需要说明的是，在不冲突的情况下，本申请的实施例及实施例中的特征可以相互组合。在下面的描述中阐述了很多具体细节以便于充分理解本发明，但是，本发明还可以采用其他不同于在此描述的其他方式来实施，因此，本发明的保护范围并不受下面公开的具体实施例的限制。首先参考图I和图2，图I示出了根据本发明的实施例的掘进机位姿检测系统的示意图，图2示出了根据本发明的实施例的掘进机位姿检测系统安装于掘进机的示意图。如图I所示和图2所示，根据本发明的实施例的掘进机位姿检测系统，包括激光指向单元206、激光幕布式标靶102、第一采集单元104、第二采集单元106和数据采集模块204，其中，激光指向单元安装在煤矿井下的巷道上方，发射激光束；激光幕布式标靶102安装在掘进机本体上，接收激光指向单元发射的激光束；第一采集单元104采集激光幕布式标靶所呈现的激光点图像并将激光点图像数据传送至数据采集模块204 ;第二采集单元106安装在掘进机上，检测第二采集单元106相对于预定坐标系的角度信息并将角度信息发送至数据采集模块204 ;数据采集模块204接收来自第一采集单元104和第二采集单元106的检测结果，根据检测结果确定掘进机的位姿参数。激光指向单元为掘进机确定行走方向的基准，为了避免人工观察激光的指向，可在掘进机上安装激光幕布式标靶，来确定激光相对于掘进机的位置，从而可确定掘进机相对于激光光束的位置，并且还通过第二采集单元来采集自身相对于预定坐标系的角度信息，从而可确定掘进机相对于预定坐标系的角度信，通过上述两个信息便可准确确定掘进机的位姿参数，并且涉及的元器件结构简单，进一步提高了系统的稳定性。优选的，如图I所示，上述数据采集模块204包括图像处理单元108和数据处理单元110，其中，图像处理单元108连接至第一采集单元104，根据激光点图像数据计算出掘进机的水平偏向位移和垂直偏向位移，并将水平偏向位置和垂直偏向位移的数据传送至数据处理单元110 ;数据处理单元110接至图像处理单元108和第二采集单106元，根据图像处理单元108的图像处理结果和第二采集单元106的检测结果得到掘进机的位姿参数，位姿参数包括偏向角、俯仰角、滚动角、水平偏向位移和垂直偏向位移。图像处理单兀108可以为目前应用在图像处理技术上的图像处理芯片，例如ARM9芯片，处理速度快且实时性强，不会因为延迟而导致掘进机碰壁。数据处理单元110包括C51单片机，该单片机结构简单，实时性强。 根据采集的激光点图像便可以知道掘进机的水平偏向位置和垂直偏向位置，以及根据第二采集单元106的检测结果便可以知道掘进机的偏向角、俯仰角、滚动角等角度信息，根据这些参数便能够准确测出掘进机相对于激光方向的位置和姿态。在上述任一技术方案中，优选的，数据处理单元110还连接至掘进机的控制器112，用于判断掘进机的偏向角是否大于角度警戒值，在大于角度警戒值时，将角度调整命令发送至控制器112，以调整掘进机的机身角度。在检测出掘进机的偏向角之后，进而判断该偏向角是否处于设定的角度警戒值，若大于，说明掘进机的姿态已不符合理想姿态，需调整掘进机的机身角度，因此向掘进机的控制发送控制信号，控制信号包含调整的角度范围和方向，掘进机收到该控制信号后，向执行机构发送调节命令，从而调整掘进机的机身角度。优选的，数据处理单元110还用于判断水平偏向位移是否大于水平位移警戒值，在大于水平位移警戒值时，计算出掘进机的左履带速度和右履带速度并获取左右履带速度差，将左右履带速度差传送至控制器112，以调整掘进机的左履带速度和/或右履带速度。同样的，通过水平位移可确定掘进机偏离航道的程度，当水平大于设定的水平位移警戒值，说明需调整掘进机的位移，使得激光点能打在激光标靶上的理想位置，通过控制器调整左右轮的行驶速度，使掘进机在行进的过程中可往左移或往右移，从而调节掘进机的水平位移。在上述任一技术方案中，优选的，数据处理单元110还用于在判断出水平偏向位移小于等于水平位移警戒值且偏向角大于角度警戒值时，将角度调整指令发送至控制器，以调整掘进机的机身角度。在判断时还需综合考虑偏向角和水平位移，虽然水平位移小于水平位移警戒值，但若偏向角仍大于角度警戒值，则仍需调整掘进机的机身角度。在上述任一技术方案中，优选的，还可以包括显示器(图中未示出)，连接至数据采集模块，显示掘进机的位姿参数。方便操作人员清楚掘进机的当前位置和姿态并进行相应的操作，当发生危险时，该显示器还可以发出警报提示。如图2所示，该掘进机位姿检测系统还可以包括固定装置202，设置在掘进机上并对准掘进机的中轴线，其中，固定装置202包括第一侧面和第二侧面，第一侧面与第二侧面均垂直于中轴线，激光幕布式标靶102安装在第一侧面上，数据采集模块204和第一采集单元安装第二侧面上并对准激光标靶102，第二采集单元106设置在数据采集模块204的上方。通过该固定装置202将相应的元器件固定在掘进机的机身上，且能够使激光打在激光标靶102上，调整激光标靶102相对于激光的理想位置，这样，才可通过激光点的位置信息来准确调整掘进机的位移，使掘进机沿着激光方向行走。在上述任一技术方案中，优选的，数据米集模块204还用于在第一米集单兀104未采集到激光点图像或激光点图像出现异常时，向掘进机的控制器112发送停止指令，使掘进机停止前进。由于突发情况可能会造成激光点脱靶、突然消失或时断时续，当检测到激光标革El 102上没有激光点时，向掘进机发送停止命令,掘进机收到该命令后停止前进,防止掘进机偏离航道而碰到巷道壁，进而出现危险。直至在靶面上检测到激光点后，再次向掘进机发送工作命令。在上述任一技术方案中，优选的，第一采集单元104包括CXD摄像头，第二采集单元106包括航姿仪。利用航姿仪测量精度高，结构简单，煤矿井下环境恶劣，利用CXD摄像头可防止受污染。本发明的还提供了一种掘进机，包括如上述任一技术方案中的掘进机位姿检测系统。 下面结合图3来详细说明掘进机位姿检测的流程图。如图3所示，在步骤302，判断是否接收到掘进机就绪指令，若接收到该指令，则进行数据采集，若没有收到该指令，则继续等待该指令。在步骤304，一方面，判断是否接收到航姿仪的采集数据，若收到，则进入步骤306，从采集数据中提取角度信息。在步骤308，根据提取的角度信息计算掘进机的偏向角、俯仰角和滚动角。在步骤310，另一方面，判断是否接收到摄像头的采集数据，若收到，则进入步骤312，从摄像头的采集数据中提取激光点坐标信息。在步骤314，根据激光点坐标信息计算掘进机的水平偏向位移和竖直偏向位移。在步骤316，通过控制算法利用水平偏向位移计算掘进机的左右履带速度和推导出实际行走方向，控制算法具体参见图4。左右履带速度的计算公式如下VL=Vset+kc (ScX Sc_khX ScXSh)；Ve = Vset-kc (ScXSc-khX S。X Sh)，其中，' 是指返回的机器人左履带实际行走速度，Vk指返回的机器人右履带实际行走速度，S。是指激光测出的实时水平位移，Sh是指激光测出的上一时刻的水平位移，k。为调整力度系数，kh为惯性抑制系数，Vsrt是预先设置的速度。在步骤318，通过CAN总线向掘进机发送角度信息、位移信息和行走控制指令，以调整掘进机的位姿。下面结合图4来详细说明调整掘进机位姿的流程。如图4所示，在步骤402，初始化航姿仪和摄像头。在步骤404，从采集的信息中获取掘进机的偏向角和水平偏向位移。在步骤406，判断偏向角是否大于角度警戒值，若大于，则进入角度突发情况处理模式，姿态检测系统将控制命令传送给掘进机控制器，掘进机控制器接收到该命令后执行原地转向命令，调整机身角度，使角度偏移量变小；若小于角度警戒值，则姿态检测系统将左右履带全速前进指令发送给控制器，掘进机全速前进并进入步骤408。在步骤408，判断水平位移是否大于水平位移警戒值，若大于，则进入位移突发情况处理模式，姿态检测系统调整左右履带速度差并传送给掘进机控制器，掘进机在行进过程中调整偏向位移；若小于水平位移警戒值，则使掘进机进行原地角度调整，角度调整后，掘进机全速前进并进入步骤410。需说明的是，当掘进机的水平位移小于水平位移警戒值，但偏向角仍大于角度警戒值时，需原地调整掘进机的角度，从而保证掘进机沿着既定路线行走。在步骤410，利用公 式计算左右履带速度。在步骤412，将行走指令发送至执行机构。如图5所示，轨迹A-B-C-D是算法执行后掘进机的运行轨迹。在A-B段时，掘进机502的右履带速度大于左履带速度，目的是让掘进机502做转向动作，且随着水平位移(SP偏离激光方向的位移)的增大，转向力度越大；在B-C-D段时，左履带速度逐渐增大，最后掘进机502处于右履带速度小于左履带速度的状态，目的是在水平位移趋向于零(即掘进机回到既定路线上)时，掘进机502的运动方向与激光方向夹角处在一个可控状态(运动方向的夹角与激光方向夹角不宜过大)。图中，504是隧道，经过调整后，掘进机502可沿着隧道按照既定路线行走。以上结合附图详细说明了根据本发明的技术方案，可准确检测出掘进机的位姿参数，且结构设计简单，系统结构稳定性高，其次可根据获取的位姿参数调整掘进机的轨迹，使得掘进机能够与既定路线保持一致，以上所述仅为本发明的优选实施例而已，并不用于限制本发明，对于本领域的技术人员来说，本发明可以有各种更改和变化。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。
权利要求
1.一种掘进机位姿检测系统，其特征在于，包括激光指向单元、激光幕布式标靶、第一采集单元、第二采集单元和数据采集模块，其中， 所述激光指向单元安装在煤矿井下的巷道上方，发射激光束； 所述激光幕布式标靶安装在掘进机上，接收所述激光指向单元发射的激光束； 所述第一采集单元采集所述激光标靶所呈现的激光点图像并将激光点图像数据传送至所述数据采集模块； 所述第二采集单元安装在所述掘进机本体上，检测所述第二采集单元相对于预定坐标系的角度信息并将所述角度信息发送至所述数据采集模块； 所述数据采集模块接收来自所述第一采集单元和所述第二采集单元的检测结果，根据所述检测结果确定所述掘进机的位姿参数。
2.根据权利要求I所述的掘进机位姿检测系统，其特征在于，所述数据采集模块包括图像处理单元和数据处理单元，其中， 所述图像处理单元连接至所述第一采集单元，根据所述激光点图像数据计算出所述掘进机的水平偏向位移和垂直偏向位移，并将所述水平偏向位置和垂直偏向位移的数据传送至所述数据处理单元； 所述数据处理单元接至所述图像处理单元和所述第二采集单元，根据所述图像处理单元的图像处理结果和所述第二采集单元的检测结果得到所述掘进机的位姿参数，所述位姿参数包括偏向角、俯仰角、滚动角、水平偏向位移和垂直偏向位移。
3.根据权利要求2所述的掘进机位姿检测系统，其特征在于，所述数据处理单元还连接至所述掘进机的控制器，用于判断所述掘进机的偏向角是否大于角度警戒值，在大于所述角度警戒值时，将角度调整命令发送至所述掘进机控制器，利用掘进机行走电磁阀开度以调整所述掘进机的机身角度。
4.根据权利要求3所述的掘进机位姿检测系统，其特征在于，所述数据处理单元还用于判断所述水平偏向位移是否大于水平位移警戒值，在大于所述水平位移警戒值时，计算出所述掘进机的左履带速度和右履带速度并获取左右履带的速度差，将所述左右履带的速度差传送至所述控制器，以调整所述掘进机的履带速度和/或右履带速度。
5.根据权利要求4所述的掘进机位姿检测系统，其特征在于，所述数据处理单元还用于在判断出所述水平偏向位移小于等于所述水平位移警戒值且所述偏向角大于所述角度警戒值时，将角度调整指令发送至所述控制器，以调整所述掘进机的机身角度。
6.根据权利要求2所述的掘进机位姿检测系统，其特征在于，还包括显示器，连接至所述数据采集模块，显示所述掘进机的位姿参数。
7.根据权利要求I所述的掘进机位姿检测系统，其特征在于，还包括固定装置，设置在所述掘进机上并对准所述掘进机的中轴线，其中，所述固定装置包括第一侧面和第二侧面，所述第一侧面与所述第二侧面均垂直于所述中轴线，所述激光幕布式标靶安装在所述第一侧面上，所述第一采集单元安装所述第二侧面上并对准所述激光幕布式标靶。
8.根据权利要求I至7中任一项所述的掘进机位姿检测系统，其特征在于，所述数据采集模块还用于在所述第一采集单元未采集到所述激光点图像或所述激光点图像出现异常时，向所述掘进机的控制器发送停止指令，使所述掘进机停止前进。
9.根据权利要求I至7中任一项所述的掘进机位姿检测系统，其特征在于，所述第一采集单元包括(XD摄像头,所述第二采集单元包括航姿仪。
10.一种掘进机，其特征在于，包括如权利要求I至9中任一项所述的掘进机位姿检测系统。
全文摘要
本发明提供了一种掘进机位姿检测系统，包括激光指向单元、激光幕布式标靶、第一采集单元、第二采集单元和数据采集模块，激光指向单元安装在煤矿井下的巷道上方，发射激光束；激光幕布式标靶安装在掘进机上，接收激光指向单元发射的激光束；第一采集单元采集激光幕布式标靶所呈现的激光点图像；第二采集单元安装在掘进机本体上，检测第二采集单元相对于预定坐标系的角度信息；数据采集模块接收来自第一采集单元和第二采集单元的检测结果，根据检测结果确定掘进机的位姿参数。根据本发明的技术方案，可准确检测出掘进机的位姿参数，准确度高，且结构设计简单，系统结构稳定性高。本发明还提出了一种掘进机。
文档编号E21C35/24GK102878976SQ20121036339
公开日2013年1月16日 申请日期2012年9月26日 优先权日2012年9月26日
发明者易定忠, 刘召安, 谷晓黎 申请人:三一重型装备有限公司