CN112746812A - 凿岩台车照明摄像随动***、控制方法及凿岩台车 - Google Patents

Abstract

本发明公开了一种凿岩台车照明摄像随动***、控制方法及凿岩台车，属于工程机械控制领域。包括车架、设置在车架上的凿岩工作组件、云台和控制装置，云台上的设置有照明摄像装置、凿岩工作组件设置有凿岩机；凿岩工作组件包括依次连接的第一构件、第二构件、第三构件、第四构件、第五构件、第六构件和第七构件；凿岩机设置在第七构件上；控制装置与云台连接。通过实时获取凿岩工作组件的各构件的空间运动信息，利用空间变换方法，实时获取凿岩机的凿岩端的空间位置，然后计算照明摄像装置的调整角度，控制云台动作实现照明摄像装置与凿岩机的凿岩端随动，照明摄像效果好，有效降低了操作人员的工作负担，提高了作业效率。

Description

凿岩台车照明摄像随动***、控制方法及凿岩台车

技术领域

本发明属于工程机械控制领域，具体涉及一种凿岩台车照明摄像随动***、控制方法及凿岩台车。

背景技术

凿岩台车(以下简称“车辆”)的凿岩装置安装在车辆大臂前侧，其在地下工作面工作时，需要大臂移动，使凿岩装置到达工作点，然后接通动力源，进行凿岩作业。由于工作点距离车辆较远，且地下光线昏暗，需要车辆前侧安装工作照明装置，使车辆凿岩工作时，工作点获得充分的光照，操作者可在车辆附近观测到凿岩装置工作情况，此外为了能够对凿岩情况进行监控，还会在车辆前侧安装摄像机，从而实施对凿岩情况进行拍摄监控。

目前，凿岩台车在作业过程中，当某个工作点凿岩结束后，需要移动大臂至另一工作点，如果新工作点和原有工作点距离较远，则操作者需要挪动照明装置以机摄像机的角度，使新工作点得到充分的光线，并且能够对新工作点进行摄像。当一个工作面的工作点较多时，需要操作者不停的人工扳动调整照明装置及摄像机方向，从而增加了操作人员的负担。此外车辆的大臂较长，操作者距离工作点较远，通过目测不易观察到较远的工作点，使得人为调整照明装置和摄像机的方向时也会不准确，导致照明和摄像的效果不好。因此为了是工作面或工作点获得更理想的光照以及拍摄效果，并能减少操作人员的工作负担，是目前凿岩台车技术领域需要解决的问题。

发明内容

技术问题：本发明针对目前凿岩台车在作业时，需要人为调整照明和摄像装置的方向，导致照明摄像效果不佳以及极大增加操作人员的工作负担的问题，本发明提供一种岩台车照明摄像随动***及相应的控制方法，提高照明和摄像的效果，并有效地减少操作人员的工作负担；进一步地，提供一种凿岩台车。

技术方案：本发明的凿岩台车照明摄像随动***，包括车架、设置在车架上的凿岩工作组件、云台和控制装置，所述云台上的设置有照明摄像装置、所述凿岩工作组件设置有凿岩机；

所述凿岩工作组件包括依次连接的第一构件、第二构件、第三构件、第四构件、第五构件、第六构件和第七构件，其中，第一构件铰接在车架上，第二构件与第一构件铰接，第三构件与第二构件滑动套接，第四构件与第三构件铰接，第五构件与第四构件铰接，第六构件与第五构件转动连接，第七构件铰接在第六构件上；

所述凿岩机设置在第七构件上；

所述控制装置与云台连接，通过实时获取凿岩工作组件的各构件的空间运动信息，控制云台动作实现照明摄像装置与凿岩机的凿岩端随动。

进一步地，所述第六构件与第五构件的轴线垂直。

进一步地，所述控制装置包括：

第一角度检测模块，用于检测第一构件的旋转角度；

第二角度检测模块，用于检测第二构件的旋转角度；

第三角度检测模块，用于检测第四构件的旋转角度；

第四角度检测模块，用于检测第五构件的旋转角度；

第五角度检测模块，用于检测第六构件的旋转角度；

第六角度检测模块，用于检测第七构件的旋转角度；

第一长度检测模块，用于检测凿岩机的凿岩端的伸长距离；

第二长度检测模块，用于检测第三构件的伸长距离；

控制器，用于根据各检测模块发送来的信息进行运算，并向云台发送控制指令，通过云台驱动照明摄像装置运动。

进一步地，所述第一角度检测模块、第二角度检测模块、第三角度检测模块、第四角度检测模块、第五角度检测模块、第五角度检测模块均为编码器。

进一步地，所述第一长度检测模块和第二长度检测模块均为拉绳位移传感器。

本发明的凿岩台车照明摄像随动控制方法，包括：

第一角度检测模块检测检测第一构件的旋转角度，第二角度检测模块检测检测第二构件的旋转角度，第三角度检测模块检测第四构件的旋转角度，第四角度检测模块检测第五构件的旋转角度，第五角度检测模块检测第六构件的旋转角度；第六角度检测模块检测第七构件的旋转角度，第一长度检测模块检测凿岩机的凿岩端的伸长距离，第二长度检测模块检测第三构件的伸长距离；

控制器根据各检测模块所检测的数据，计算凿岩机的凿岩端的空间位置，根据凿岩端的空间位置计算照明摄像装置的运动角度；

控制器根据照明摄像装置的运动角度，向云台发送控制指令，控制云台驱动照明摄像装置与凿岩机的凿岩端随动。

进一步地，计算凿岩机的凿岩端的空间位置的方法为：

以照明摄像装置的安装点J0为坐标原点，构建全局坐标系Coord0；

以第一构件与车架的连接点J1为坐标原点，构建第一局部坐标系Coord1，所述第一局部坐标系Coord1固定在第一构件上；

以第一构件和第二构件的连接点J2为坐标原点，构建第二局部坐标系Coord2，所述第二局部坐标系Coord2固定在第二构件上；

以第三构件与第四构件连接的一端的端点J3为坐标原点，构建第三局部坐标系Coord3，所述第三局部坐标系Coord3固定在第三构件上；

以第三构件和第四构件的连接点J4为坐标原点，构建第四局部坐标系Coord4，所述第四局部坐标系Coord4固定在第四构件上；

以第四构件和第五构件的连接点J5为坐标原点，构建第五局部坐标系Coord5，所述第五局部坐标系Coord5固定在第五构件上；

以第五构件与第六构件的连接点J6为坐标原点，构建第六局部坐标系Coord6，所述第六局部坐标系Coord6固定在第六构件上；

以第六构件与第七构件的连接点J7为坐标原点，构建第七局部坐标系Coord7，所述第七局部坐标系Coord7固定在第七构件上；

根据所构建的坐标系Coord0、Coord1、Coord2、Coord3、Coord4、Coord5、Coord6、Coord7，利用空间变换方法，计算凿岩机的凿岩端的在全局坐标系下的空间坐标；

根据凿岩机的凿岩端的在全局坐标系下的空间坐标确定照明摄像装置的调整角度。

进一步地，利用空间变换方法，计算凿岩机的凿岩端的初始位置在全局坐标系下的空间坐标的方法为：

确定凿岩机的凿岩端在第七局部坐标系Coord7下的初始位置的齐次坐标P₀，根据凿岩机的凿岩端的伸长距离，计算第一平移变换矩阵H₁，得到变换后凿岩端的齐次坐标P₁，其中，P₁＝H₁×P₀；

获取的第七构件绕第六局部坐标系Coord6的Y6轴的旋转角度θ₁，计算第一旋转变换矩阵R₁，确定坐标原点J7在第六局部坐标系Coord6中的齐次坐标P_J6，根据P_J6确定第二平移变换矩阵H₂，得到凿岩端初始位置在Coord6中的齐次坐标P₂，P₂＝H₂×R₁×P₁；

获取第六构件绕第五局部坐标系Coord5的X5轴的旋转角度θ₂，确定第二旋转变换矩阵R₂，确定坐标原点J6在第五局部坐标系Coord5中的齐次坐标P_J5，根据P_J5确定第三平移变换矩阵H₃，得到凿岩端初始位置在Coord5中的齐次坐标P₃，P₃＝H₃×R₂×P₂；

获取第五构件绕第四局部坐标系Coord4的Z4轴的旋转角度θ₃，确定第三旋转变换矩阵R₃，确定坐标原点J5在第四局部坐标系Coord4中的齐次坐标P_J4，根据P_J4确定第四平移变换矩阵H₄，得到凿岩端初始位置在Coord4中的齐次坐标P₄，P₄＝H₄×R₃×P₃；

获取第四构件绕第三局部坐标系Coord3的Y3轴的旋转角度θ₄，确定第四旋转变换矩阵R₄，确定坐标原点J4在第三局部坐标系Coord3中的齐次坐标P_J3，根据P_J3确定第五平移变换矩阵H₅，得到凿岩端初始位置在Coord3中的齐次坐标P₅，P₅＝H₅×R₄×P₄；

获取第三构件沿第二局部坐标系Coord2的X2轴方向伸长距离L₂，确定第六平移变换矩阵H₆，确定坐标原点J3在第二局部坐标系Coord2中的齐次坐标P_J2，根据P_J2确定第七平移变换矩阵H₇，得到凿岩端初始位置在Coord2中的齐次坐标P₆，P₆＝H₇×H₆×P₅；

获取第二构件绕第一局部坐标系Coord1的Y1轴的旋转角度θ₅，确定第五旋转变换矩阵R₅，确定坐标原点J2在第一局部坐标系Coord1中的齐次坐标P_J1，根据P_J1确定第八平移变换矩阵H₈，得到凿岩端初始位置在Coord1中的齐次坐标P₇，P₇＝H₈×R₅×P₆；

获取第一构件绕全局坐标系Coord0的Z轴的旋转角度θ₆，确定第六旋转变换矩阵R₆，确定坐标原点J1在全局坐标系Coord0中的齐次坐标P_J0，根据P_J0确定第九平移变换矩阵H₉，得到凿岩端初始位置在Coord0中的齐次坐标P₈，P₈＝H₉×R₆×P₇，即为凿岩端在全局坐标系下的空间坐标。

进一步地，根据凿岩机的凿岩端在全局坐标系下的空间坐标确定照明摄像装置的调整角度的方法为：

将凿岩端在全局坐标系下的空间坐标P₈表示为：

P₁＝[θ，β，γ，1]^T

计算照明摄像角度在全局坐标系Coord0的角度相对于X、Y、Z轴的角度分别为θ_x、θ_y、θ_z，则

控制器根据θ_x、θ_y、θ_z的值，通过云台控制照明摄像装置调整角度，实现随动。

本发明的凿岩台车，所述的照明摄像随动控制***，并利用所述照明摄像随动控制方法控制照明摄像装置随动。

有益效果：本发明与现有技术相比，本发明中照明摄像装置能够与凿岩机的凿岩端随动，照明摄像装置的方向能够随着凿岩机的凿岩端进行调整，使得照明方向与摄像方向能够实时对准凿岩机的凿岩端，因此避免了由于操作人员离作业点的距离过远时，不能准确调整照明摄像装置的方向，导致照明摄像效果不理想的问题；同时，照明摄像装置的方向自动调整，无需人工搬动，极大地降低了操作人员的工作负担，提高了作业效率。

附图说明

图1为本发明凿岩台车照明摄像随动***的结构示意图；

图2为图1中A处的局部放大图；

图3为本发明的工作组件的结构图；

图4为本发明的一种实施例中的凿岩机结构图；

图5为照明摄像角度示意图。

图中有：1、车架；2、云台；3、照明摄像装置；4、第一构件；5、第二构件、6、第三构件；7、第四构件；8、第五构件；9、第六构件；10、第七构件；11、凿岩机。

具体实施方式

下面结合实施例和说明书附图对本发明作进一步的说明，其中，术语“第一”、“第二”、“第三”等仅用于描述目的，而不能理解为指示或暗示相对重要性。

结合图1所示，本发明的照明摄像随动***包括车架1、设置在车架1上的凿岩工作组件、云台2和控制装置，云台2上的设置有照明摄像装置3、所述凿岩工作组件设置有凿岩机11。云台2是用于安装、固定支撑照明摄像装置3，可以利用电机来调整照明摄像装置3的水平和俯仰角度，通常照明方向和摄像方向是相同的，所以设置在同一个云台2上，从而实现照明和摄像方向的同步调整。对于云台2、照明摄像装置3机均可以采用现有的，因此，这里就不对其结构做详细说明，照明摄像机可以采用单独的结构照明灯和摄像机，也可以采用带照明灯的摄像机。

结合图1和图2所示，所述凿岩工作组件包括依次连接的第一构件4、第二构件5、第三构件6、第四构件7、第五构件8、第六构件9和第七构件10，其中，第一构件4铰接在车架1上，第二构件5与第一构件4铰接，第三构件6与第二构件5滑动套接，第四构件7与第三构件6铰接，第五构件8与第四构件7铰接，第六构件9与第五构件8转动连接，第七构件10铰接在第六构件9上。通过若干个构件的连接结构，可以实现凿岩过程的多方位作业，各构件在运动时，可以通过液压驱动、或者电机驱动的方式实现平动或转动，均可利用现有的技术完成驱动。

在本发明的实施例中，第六构件9与第五构件8的轴线垂直，便于增大凿岩台车的凿岩工作面。

凿岩机11设置在第七构件10上，当利用凿岩工作组件将凿岩机11调整到凿岩作业点后，进行凿岩，凿岩机11可以采用现有技术中的凿岩机，例如图4所示的凿岩机结构，图4为申请号201910064722.6的专利中凿岩机的结构。

控制模块与云台2连接，通过实时获取凿岩工作组件的各构件的空间运动信息，根据所获信息，控制云台2动作驱动照明摄像装置3的与凿岩机11的凿岩端随动，从而照明摄像装置3的方向能够随着凿岩机11的凿岩端进行调整，使得照明方向与摄像方向能够实时对准凿岩机11的凿岩端，因此避免了由于操作人员离作业点的距离过远时，不能准确调整照明摄像装置3的方向，导致照明摄像效果不理想的问题；同时，照明摄像装置的方向自动调整，无需人工搬动，极大地降低了操作人员的工作负担，提高了作业效率。

为了能够获得凿岩工作组件的各构件以及凿岩机11运动信息，本发明的实施例中，控制装置包括第一角度检测模块，用于检测第一构件4的旋转角度；第二角度检测模块，用于检测第二构件5的旋转角度；第三角度检测模块，用于检测第四构件7的旋转角度；第四角度检测模块，用于检测第五构件8的旋转角度；第五角度检测模块，用于检测第六构件9的旋转角度；第六角度检测模块，用于检测第七构件10的旋转角度；第一长度检测模块，用于检测凿岩机11的凿岩端的伸长距离；第二长度检测模块，用于检测第三构件6的伸长距离；控制器，用于根据各检测模块发送来的信息进行运算，并向云台2发送控制指令，通过云台2驱动照明摄像装置3运动。

具体的，在发明的实施例中，角度检测模块均采用编码器，编码器可以将角位移转换为电信号，并能将电信号进行编制、转换为可用以通讯、传输和存储，从而能够对相应部件的转动角度进行检测。两个长度检测模块采用的是拉绳位移传感器，能够测量相应构件的伸缩变化。控制器可采用PLC，PLC能够根据各检测模块发来的数据，计算凿岩机11的凿岩端的空间位置，然后根据所计算的空间位置，实时计算照明灯和摄像机的俯仰角度，然后向云台2发送控制信号，云台2根据所接收的信号驱动照明摄像装置3运动，实现照明摄像装置与凿岩机11的凿岩端随动。

基于本发明的凿岩台车照明摄像随动***，提出了一种凿岩台车照明摄像随动控制方法，该方法包括第一角度检测模块检测检测第一构件4的旋转角度，第二角度检测模块检测检测第二构件5的旋转角度，第三角度检测模块检测第四构件7的旋转角度，第四角度检测模块检测第五构件8的旋转角度，第五角度检测模块检测第六构件9的旋转角度；第六角度检测模块检测第七构件10的旋转角度，第一长度检测模块检测凿岩机11的凿岩端的伸长距离，第二长度检测模块检测第三构件6的伸长距离；控制器根据各检测模块所检测的数据，计算凿岩机11的凿岩端的空间位置，根据凿岩端的空间位置计算照明摄像装置3的运动角度；控制器根据照明摄像装置3的运动角度，向云台2发送控制指令，控制云台2驱动照明摄像装置3与凿岩机11的凿岩端随动。

为了计算计算凿岩机11的凿岩端的空间位置，首先需要建立空间坐标系，由于凿岩工作组件包括多个运动构件，因此需要建立全局坐标系和各固定在各运动构件上的局部坐标系，然后利用空间变换方法，计算凿岩机11的凿岩端在全局坐标系的空间坐标，即获得凿岩端的空间位置。

建立空间坐标系的方法为：以照明摄像装置3的安装点J0为坐标原点，构建全局坐标系Coord0，全局坐标系Coord0的坐标轴方向如图1中的X、Y、Z方向，在图1中，Y轴垂直纸面。

以第一构件4与车架的连接点J1为坐标原点，构建第一局部坐标系Coord1，第一局部坐标系Coord1固定在第一构件4上，Coord1的坐标轴方向如图3中的X1、Y1、Z1方向。

以第一构件4与第二构件5的连接点J2为坐标原点，构建第二局部坐标系Coord2，第二局部坐标系Coord2固定在第二构件5上，Coord2的坐标轴方向如图3中的X2、Y2、Z2方向。

以第三构件6与第四构件7连接一端的端点J3为坐标原点，构建第三局部坐标系Coord3，第三局部坐标系Coord3固定在第三构件6上，Coord3的坐标轴方向如图3中的X3、Y3、Z3方向。

以第三构件6与第四构件7的连接点J4为坐标原点，构建第四局部坐标系Coord4，第四局部坐标系Coord4固定在第四构件7上，Coord4的坐标轴方向如图3中的X4、Y4、Z4方向。

以第四构件7与第五构件8的连接点J5为坐标原点，构建第五局部坐标系Coord5，第五局部坐标系Coord5固定在第五构件8上，Coord5的坐标轴方向如图3中的X5、Y5、Z5方向。

以第五构件8与第六构件9的连接点J6为坐标原点，构建第六局部坐标系Coord6，第六局部坐标系Coord6固定在第六构件9上，Coord6的坐标轴方向如图3中的X6、Y6、Z6方向。

以第六构件9与第七构件10的连接点J7为坐标原点，构建第七局部坐标系Coord7，第七局部坐标系Coord7固定在第七构件10上，Coord7的坐标轴方向如图3中的X7、Y7、Z7方向。

根据所构建的坐标系Coord0、Coord1、Coord2、Coord3、Coord4、Coord5、Coord6、Coord7，利用空间变换方法，即可计算出凿岩机11的凿岩端的在全局坐标系Coord0下的空间坐标。具体的计算方法为：

确定凿岩机11的凿岩端在第七局部坐标系Coord7下的初始位置的齐次坐标P₀＝[X₇，Y₇，Z₇，1]^T，根据第一长度检测模块获得的凿岩端沿Coord7的X7轴的伸长距离L₁，计算第一平移变换矩阵H₁，第一平移变换矩阵H₁为：

得到变换后凿岩端的齐次坐标P₁，其中，P₁＝H₁×P₀。

根据第六角度检测模块获取的第七构件10绕第六局部坐标系Coord6的Y6轴的旋转角度θ₁，计算第一旋转变换矩阵R₁为：

其中C表示cos(()余弦函数，S表示sin()正弦函数。

确定坐标原点J7在第六局部坐标系Coord6中的齐次坐标P_J6＝[X₆，Y₆，Z₆，1]^T，因为第六构件9的长度是确定的，因此可以直接获得P_J6，其中，在本发明的实施例中，可知X₆和Z₆均为0，Y₆为J6到J7的距离。根据P_J6确定第二平移变换矩阵H₂为：

并计算得到凿岩端初始位置在Coord6中的齐次坐标P₂为：

P₂＝H₂×R₁×P₁＝H₂×R₁×H₁×P₀

利用第五角度传感器获取第六构件9绕第五局部坐标系Coord5的X5轴的旋转角度θ₂，确定第二旋转变换矩阵R₂为：

确定坐标原点J6在第五局部坐标系Coord5中的齐次坐标P_J5＝[X₅，Y₅，Z₅，1]^T，其中，在本发明的实施例中Y₅、Z₅均为0，X₅为J5到J6的距离。根据P_J5确定第三平移变换矩阵H₃为：

计算得到凿岩端初始位置在Coord5中的齐次坐标P₃：

P₃＝H₃×R₂×P₂＝H₃×R₂×H₂×R₁×H₁×P₀。

利用第四角度检测模块获取第五构件8绕第四局部坐标系Coord4的Z4轴的旋转角度θ₃，确定第三旋转变换矩阵R₃为：

确定坐标原点J5在第四局部坐标系Coord4中的齐次坐标P_J4＝[X₄，Y₄，Z₄，1]^T，其中，在本发明的实施例中Y₄、Z₄均为0，X₄为J4到J5的距离。根据P_J4确定第四平移变换矩阵H₄为：

计算得到凿岩端初始位置在Coord4中的齐次坐标P₄，

P₄＝H₄×R₃×P₃＝H₄×R₃×H₃×R₂×H₂×R₁×H₁×P₀

利用第三角度检测模块获取第四构件7绕第三局部坐标系Coord3的Y3轴的旋转角度θ₄，确定第四旋转变换矩阵R₄为：

确定坐标原点J4在第三局部坐标系Coord3中的齐次坐标P_J3＝[X₃，Y₃，Z₃，1]^T，其中，在本发明的实施例中，Y₃、Z₃均为0，X₃为J3到J4的距离。根据P_J3确定第五平移变换矩阵H₅为：

计算得到凿岩端初始位置在Coord3中的齐次坐标P₅：

P₅＝H₅×R₄×P₄＝H₅×R₄×H₄×R₃×H₃×R₂×H₂×R₁×H₁×P₀

利用第二长度检测模块获取第三构件6沿第二局部坐标系Coord2的X2轴方向伸长距离L₂，确定第六平移变换矩阵H₆为：

确定坐标原点J3在第二局部坐标系Coord2中的齐次坐标P_J2＝[X₂，Y₂，Z₂，1]^T，在本发明的实施例中，Y₂、Z₂均为0，X₂为J2到J3的距离。根据P_J2确定第七平移变换矩阵H₇为：

计算得到凿岩端初始位置在Coord2中的齐次坐标P₆：

P₆＝H₇×H₆×P₅＝H₇×H₆×H₅×R₄×H₄×R₃×H₃×R₂×H₂×R₁×H₁×P₀

利用第二角度检测模块获取第二构件5绕第一局部坐标系Coord1的Y1轴的旋转角度θ₅，确定第五旋转变换矩阵R₅为：

确定坐标原点J2在第一局部坐标系Coord1中的齐次坐标P_J1＝[X₁，Y₁，Z₁，1]^T，在本发明的实施例中，Y₁、Z₁均为0，X₁为J1到J2的距离。根据P_J1确定第八平移变换矩阵H₈为：

计算得到凿岩端初始位置在Coord1中的齐次坐标P₇：

P₇＝H₈×R₅×P₆

＝H₈×R₅×H₇×H₆×H₅×R₄×H₄×R₃×H₃×R₂×H₂×R₁×H₁×P₀

利用第一角度检测获取第一构件4绕全局坐标系Coord0的Z轴的旋转角度θ₆，确定第六旋转变换矩阵R₆为：

确定坐标原点J1在全局坐标系Coord0中的齐次坐标P_J0＝[X₀，Y₀，Z₀，1]^T，即J1点在全局坐标系下的坐标。根据P_J0确定第九平移变换矩阵H₉为：

计算得到凿岩端初始位置在Coord0中的齐次坐标P₈：

P₈＝H₉×R₆×P₇

＝H₈×R₅×H₇×H₆×H₅×R₄×H₄×R₃×H₃×R₂×H₂×R₁×H₁×P₀

根据P₈来计算照明摄像的角度，具体的，将凿岩端在全局坐标系下的空间坐标P₈表示为：

P₈＝[α，β，γ1]^T

如图5所示，照明摄像角度在全局坐标系Coord0的角度相对于X、Y、Z轴的角度分别为θ_x、θ_y、θ_z，则：

利用上述方法，PLC实时计算得到照明摄像装置3的调整角度，然后向云台2发送控制信号，云台2驱动照明摄像装置3运动，使得照明和摄像方向实时对准凿岩机11的凿岩端，实现了照明和摄像的随动。无需人工进行角度调整，使得照明和摄像效果好，并且极大降低了操作人员的负担，自动化程度高，提高了凿岩台车作业效率。

进一步地，提供了一种凿岩台车，该凿岩台车采用了所提出的照明摄像随动***，并利用所提出的控制方法进行随动控制，从而使得操作人员在驾驶凿岩台车作业时，能够更好地对作业面或作业点进行照明和摄像，较少了操作人员的工作负担，提高了凿岩作业效率。

上述实施例仅是本发明的优选实施方式，应当指出：对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和等同替换，这些对本发明权利要求进行改进和等同替换后的技术方案，均落入本发明的保护范围。

Claims (10)

1.一种凿岩台车照明摄像随动***，其特征在于，包括车架(1)、设置在车架(1)上的凿岩工作组件、云台(2)和控制装置，所述云台(2)上的设置有照明摄像装置(3)、所述凿岩工作组件设置有凿岩机(11)；

所述凿岩工作组件包括依次连接的第一构件(4)、第二构件(5)、第三构件(6)、第四构件(7)、第五构件(8)、第六构件(9)和第七构件(10)，其中，第一构件(4)铰接在车架(1)上，第二构件(5)与第一构件(4)铰接，第三构件(6)与第二构件(5)滑动套接，第四构件(7)与第三构件(6)铰接，第五构件(8)与第四构件(7)铰接，第六构件(9)与第五构件(8)转动连接，第七构件(10)铰接在第六构件(9)上；

所述凿岩机(11)设置在第七构件(10)上；

所述控制装置与云台(2)连接，通过实时获取凿岩工作组件的各构件的空间运动信息，控制云台(2)动作实现照明摄像装置(3)与凿岩机(11)的凿岩端随动。

2.根据权利要求1所述的凿岩台车照明摄像随动***，其特征在于，所述第六构件(9)与第五构件(8)的轴线垂直。

3.根据权利要求1所述的凿岩台车照明摄像随动***，其特征在于，所述控制装置包括：

第一角度检测模块，用于检测第一构件(4)的旋转角度；

第二角度检测模块，用于检测第二构件(5)的旋转角度；

第三角度检测模块，用于检测第四构件(7)的旋转角度；

第四角度检测模块，用于检测第五构件(8)的旋转角度；

第五角度检测模块，用于检测第六构件(9)的旋转角度；

第六角度检测模块，用于检测第七构件(10)的旋转角度；

第一长度检测模块，用于检测凿岩机(11)的凿岩端的伸长距离；

第二长度检测模块，用于检测第三构件(6)的伸长距离；

控制器，用于根据各检测模块发送来的信息进行运算，并向云台(2)发送控制指令，通过云台(2)驱动照明摄像装置(3)运动。

4.根据权利要求3所述的凿岩台车照明摄像随动***，其特征在于，所述第一角度检测模块、第二角度检测模块、第三角度检测模块、第四角度检测模块、第五角度检测模块、第五角度检测模块均为编码器。

5.根据权利要求3或4所述的凿岩台车照明摄像随动***，其特征在于，所述第一长度检测模块和第二长度检测模块均为拉绳位移传感器。

6.一种凿岩台车照明摄像随动控制方法，其特征在于，包括：

第一角度检测模块检测检测第一构件(4)的旋转角度，第二角度检测模块检测检测第二构件(5)的旋转角度，第三角度检测模块检测第四构件(7)的旋转角度，第四角度检测模块检测第五构件(8)的旋转角度，第五角度检测模块检测第六构件(9)的旋转角度；第六角度检测模块检测第七构件(10)的旋转角度，第一长度检测模块检测凿岩机(11)的凿岩端的伸长距离，第二长度检测模块检测第三构件(6)的伸长距离；

控制器根据各检测模块所检测的数据，计算凿岩机(11)的凿岩端的空间位置，根据凿岩端的空间位置计算照明摄像装置(3)的运动角度；

控制器根据照明摄像装置(3)的运动角度，向云台(2)发送控制指令，控制云台(2)驱动照明摄像装置(3)与凿岩机(11)的凿岩端随动。

7.根据权利要求6所述的凿岩台车照明摄像随动控制方法，其特征在于，计算凿岩机(11)的凿岩端的空间位置的方法为：

以照明摄像装置(3)的安装点J0为坐标原点，构建全局坐标系Coord0；

以第一构件(4)与车架(1)的连接点J1为坐标原点，构建第一局部坐标系Coord1，所述第一局部坐标系Coord1固定在第一构件(4)上；

以第一构件(4)和第二构件(5)的连接点J2为坐标原点，构建第二局部坐标系Coord2，所述第二局部坐标系Coord2固定在第二构件(5)上；

以第三构件(6)与第四构件(7)连接的一端的端点J3为坐标原点，构建第三局部坐标系Coord3，所述第三局部坐标系Coord3固定在第三构件(6)上；

以第三构件(6)和第四构件(7)的连接点J4为坐标原点，构建第四局部坐标系Coord4，所述第四局部坐标系Coord4固定在第四构件(7)上；

以第四构件(7)和第五构件(8)的连接点J5为坐标原点，构建第五局部坐标系Coord5，所述第五局部坐标系Coord5固定在第五构件(8)上；

以第五构件(8)与第六构件(9)的连接点J6为坐标原点，构建第六局部坐标系Coord6，所述第六局部坐标系Coord6固定在第六构件(9)上；

以第六构件(9)与第七构件(10)的连接点J7为坐标原点，构建第七局部坐标系Coord7，所述第七局部坐标系Coord7固定在第七构件(10)上；

根据所构建的坐标系Coord0、Coord1、Coord2、Coord3、Coord4、Coord5、Coord6、Coord7，利用空间变换方法，计算凿岩机(11)的凿岩端的在全局坐标系下的空间坐标；

根据凿岩机(11)的凿岩端的在全局坐标系下的空间坐标确定照明摄像装置(3)的调整角度。

8.根据权利要求7所述的凿岩台车照明摄像随动控制方法，其特征在于，利用空间变换方法，计算凿岩机(11)的凿岩端的初始位置在全局坐标系下的空间坐标的方法为：

确定凿岩机(11)的凿岩端在第七局部坐标系Coord7下的初始位置的齐次坐标P₀，根据凿岩机(11)的凿岩端的伸长距离，计算第一平移变换矩阵H₁，得到变换后凿岩端的齐次坐标P₁，其中，P₁＝H₁×P₀；

获取的第七构件(10)绕第六局部坐标系Coord6的Y6轴的旋转角度θ₁，计算第一旋转变换矩阵R₁，确定坐标原点J7在第六局部坐标系Coord6中的齐次坐标P_J6，根据P_J6确定第二平移变换矩阵H₂，得到凿岩端初始位置在Coord6中的齐次坐标P₂，P₂＝H₂×R₁×P₁；

获取第六构件(9)绕第五局部坐标系Coord5的X5轴的旋转角度θ₂，确定第二旋转变换矩阵R₂，确定坐标原点J6在第五局部坐标系Coord5中的齐次坐标P_J5，根据P_J5确定第三平移变换矩阵H₃，得到凿岩端初始位置在Coord5中的齐次坐标P₃，P₃＝H₃×R₂×P₂；

获取第五构件(8)绕第四局部坐标系Coord4的Z4轴的旋转角度θ₃，确定第三旋转变换矩阵R₃，确定坐标原点J5在第四局部坐标系Coord4中的齐次坐标P_J4，根据P_J4确定第四平移变换矩阵H₄，得到凿岩端初始位置在Coord4中的齐次坐标P₄，P₄＝H₄×R₃×P₃；

获取第四构件(7)绕第三局部坐标系Coord3的Y3轴的旋转角度θ₄，确定第四旋转变换矩阵R₄，确定坐标原点J4在第三局部坐标系Coord3中的齐次坐标P_J3，根据P_J3确定第五平移变换矩阵H₅，得到凿岩端初始位置在Coord3中的齐次坐标P₅，P₅＝H₅×R₄×P₄；

获取第三构件(6)沿第二局部坐标系Coord2的X2轴方向伸长距离L₂，确定第六平移变换矩阵H₆，确定坐标原点J3在第二局部坐标系Coord2中的齐次坐标P_J2，根据P_J2确定第七平移变换矩阵H₇，得到凿岩端初始位置在Coord2中的齐次坐标P₆，P₆＝H₇×H₆×P₅；

获取第二构件(5)绕第一局部坐标系Coord1的Y1轴的旋转角度θ₅，确定第五旋转变换矩阵R₅，确定坐标原点J2在第一局部坐标系Coord1中的齐次坐标P_J1，根据P_J1确定第八平移变换矩阵H₈，得到凿岩端初始位置在Coord1中的齐次坐标P₇，P₇＝H₈×R₅×P₆；

获取第一构件(4)绕全局坐标系Coord0的Z轴的旋转角度θ₆，确定第六旋转变换矩阵R₆，确定坐标原点J1在全局坐标系Coord0中的齐次坐标P_J0，根据P_J0确定第九平移变换矩阵H₉，得到凿岩端初始位置在Coord0中的齐次坐标P₈，P₈＝H₉×R₆×P₇，即为凿岩端在全局坐标系下的空间坐标。

9.根据权利要求8所述的凿岩台车照明摄像随动控制方法，其特征在于，根据凿岩机(11)的凿岩端在全局坐标系下的空间坐标确定照明摄像装置(3)的调整角度的方法为：

将凿岩端在全局坐标系下的空间坐标P₈表示为：

P₁＝[α，β，γ，1]^T

计算照明摄像角度在全局坐标系Coord0的角度相对于X、Y、Z轴的角度分别为θ_x、θ_y、θ_z，则

控制器根据θ_x、θ_y、θ_z的值，通过云台(2)控制照明摄像装置(3)调整角度，实现随动。

10.一种凿岩台车，其特征在于，包括权利要求1-5任一项所述的照明摄像随动控制***，并利用权利要求6-9任一项所述照明摄像随动控制方法控制照明摄像装置(3)随动。

Images