本发明涉及一种采煤机姿态检测系统及方法，具体是一种基于双目视觉及惯性导航的采煤机姿态检测系统及方法。
背景技术
为了实现采矿“三机”的联动，对采煤机的空间位置及姿态进行准确检测，即对采煤机进行空间动态定位具有重要意义。为了实现采煤机位置及姿态检测,有学者提出了采煤机惯性导航定位方法。捷联惯性导航系统是指将陀螺仪和加速度计直接固定在运载体上，利用陀螺仪和加速度计等惯性敏感器件对运行载体三轴角速度和三轴加速度信息进行实时测量，结合运行载体初始惯性信息,通过高速积分获得运动载体的姿态、速度及位置等导航信息。捷联惯性导航系统在工作时不依赖外界信息，也不向外界辐射能量，不易受到干扰破坏，是一种自主式导航系统，具有数据更新率高、数据全面以及短时定位精度高等优点。
但同时，对于纯捷联惯性导航系统，也存在着一定的问题，由于采煤机的姿态信息需要进行积分得到，使其具有短时间精度高，但长时间累积会发散的特点，因此在实际系统长时间运行过程中如无法对其累积的误差进行校正，则无法满足采煤机精确定位的要求。
技术实现要素：
针对上述现有技术存在的问题，本发明提供一种基于双目视觉及惯性导航的采煤机姿态检测系统及方法，能有效减小测量采煤机姿态角的误差，提高对采煤机姿态定位误差的修正效果。
为了实现上述目的，本发明采用的技术方案是：一种基于双目视觉及惯性导航的采煤机姿态检测系统，包括防爆箱、固定支架、摄像头、激光标靶、激光指向仪、捷联惯性导航系统、处理器和供电电源；所述防爆箱和激光标靶固定安装在采煤机机身上，捷联惯性导航系统和处理器安装在防爆箱中，摄像头通过固定支架固定在液压支架上，所述摄像头为两个，激光指向仪位于采煤工作面上，激光指向仪发射十字激光，十字激光照射在激光标靶表面，两个摄像头的拍摄方向朝着激光标靶，用于采集激光标靶的图像。
进一步，所述的处理器包括微处理单元模块、通信模块、报警模块、数据存储模块及隔离电路，微处理单元模块分别与通信模块、报警模块、数据存储模块及隔离电路相连。
进一步，所述处理器中微处理单元模块是dsp芯片。
进一步，所述的供电电源采用12v直流锂电池。
进一步，所述摄像头与固定支架为铰接。
一种基于双目视觉及惯性导航的采煤机姿态检测系统的方法，具体步骤为：
a、激光指向仪发射十字激光，十字激光照在激光标靶上，两个摄像头对激光标靶的十字激光标志点进行拍摄，处理器对拍摄到的激光标靶图像先进行图像二值化处理，然后通过设定阈值的方式检测标志点所处的位置，进而得到标志点的二维图像坐标；基于双目视觉原理，两个摄像头对激光标靶上的十字激光点进行实时检测并跟踪，利用基于顺序一致性的极线约束匹配算法对检测到的标志点进行立体匹配，计算得出标志点的三维坐标，从而得到采煤机的姿态信息，达到对采煤机姿态的实时解算；
b、利用捷联惯性导航系统中的陀螺仪测出的地球自转角速度，加速度计测出的重力加速度，采用解析法初始对准求得采煤机的初始姿态角，将采煤机的初始姿态角转化为四元数后，利用捷联惯性导航系统得到采煤机机身的实时角速度和加速度，并经过圆锥误差补偿后采用姿态更新的三子样算法实时计算采煤机的姿态角；
c、利用卡尔曼滤波实现惯性导航和视觉信息的融合，综合考虑系统的精度和实时性，系统考虑全部状态的7维卡尔曼滤波器如下：
z＝hax+v
其中x＝[δveδvnδlδλφxφyφz]，其中δve为东向速度误差、δvn为北向速度误差、δl为经度误差、δλ为纬度误差、φx、φy、φz分别为失准角，w和v为系统噪声和观测噪声，分别为零均值的高斯白噪声；
根据建立的状态方程和量测方程，将状态方程和量测方程离散化之后，即能建立惯性导航/视觉的闭环卡尔曼滤波方程，将卡尔曼滤波方程得到的状态进行状态反馈后，即得到精确的采煤机姿态角。
与现有技术相比，本发明通过将惯性导航测得的姿态信息和双目视觉测得的姿态信息相结合的方式，通过对双目视觉采集的图像中检测到的标志点进行立体匹配，保证对采煤机的位置与状态确定的精度；由于捷联惯性导航系统在工作时不依赖外界信息，也不向外界辐射能量，不易受到干扰破坏，是一种自主式导航系统，因此具有数据更新率高、数据全面以及短时定位精度高等优点；同时将双目视觉系统与捷联惯性导航系统进行融合，可以进一步减小采煤机姿态角的误差，提高了对采煤机姿态定位误差的修正效果。
附图说明
图1是本发明的整体结构示意图；
图2是本发明中防爆箱内的布设示意图；
图3是本发明中基于双目数据得到采煤机的姿态的流程图；
图4是本发明中通过捷联惯性导航得到的采煤机姿态的流程图；
图5是本发明中惯性导航和视觉信息的融合运行流程图。
图中，1、采煤机，2、防爆箱，3、液压支架，4、固定支架，5、摄像头，6、激光标靶，7、激光指向仪，8、捷联惯性导航系统，9、处理器，10、供电电源。
具体实施方式
下面将对本发明作进一步说明。
如图1和2所示，一种基于双目视觉及惯性导航的采煤机姿态检测系统，包括防爆箱2、固定支架4、摄像头5、激光标靶6、激光指向仪7、捷联惯性导航系统8、处理器9和供电电源10；所述防爆箱2和激光标靶6固定安装在采煤机1机身上，捷联惯性导航系统8和处理器9安装在防爆箱2中，摄像头5通过固定支架4固定在液压支架3上，所述摄像头为两个，激光指向仪7位于采煤工作面上，激光指向仪7发射十字激光，十字激光照射在激光标靶6表面，两个摄像头5的拍摄方向朝着激光标靶6，用于采集激光标靶6的图像。处理器9用于处理捷联惯性导航系统8的加速度信号和角速度信号，以及摄像头5所拍摄的激光标靶6的图像。
进一步，所述的处理器9包括微处理单元模块、通信模块、报警模块、数据存储模块及隔离电路，微处理单元模块分别与通信模块、报警模块、数据存储模块及隔离电路相连。
进一步，所述处理器9中微处理单元模块是ti公司的dsp芯片。该芯片不仅有很高的代码效率，同时可以大大降低系统成本。
进一步，所述的供电电源10采用12v直流锂电池。用于对捷联惯性导航系统8和处理器9供电。
进一步，所述摄像头5与固定支架4为铰接，能使摄像头5相对固定支架转动，如此设置可以使摄像头5最大限度地看到最广的范围，拍摄效果好、无死角。
如图3至图5所示，一种基于双目视觉及惯性导航的采煤机姿态检测系统的方法，具体步骤为：
a、激光指向仪7发射十字激光，十字激光照在激光标靶6上，两个摄像头5对激光标靶6的十字激光标志点进行拍摄，处理器9对拍摄到的激光标靶图像先进行图像二值化处理，然后通过设定阈值的方式检测标志点所处的位置，进而得到标志点的二维图像坐标；基于双目视觉原理，两个摄像头5对激光标靶6上的十字激光点进行实时检测并跟踪，利用基于顺序一致性的极线约束匹配算法对检测到的标志点进行立体匹配，计算得出标志点的三维坐标，从而得到采煤机1的姿态信息，达到对采煤机1姿态的实时解算；
b、利用捷联惯性导航系统8中的陀螺仪测出的地球自转角速度，加速度计测出的重力加速度，采用解析法初始对准求得采煤机1的初始姿态角，将采煤机1的初始姿态角转化为四元数后，利用捷联惯性导航系统8得到采煤机1机身的实时角速度和加速度，并经过圆锥误差补偿后采用姿态更新的三子样算法实时计算采煤机1的姿态角；
c、利用卡尔曼滤波实现惯性导航和视觉信息的融合，综合考虑系统的精度和实时性，系统考虑全部状态的7维卡尔曼滤波器如下：
z＝hax+v
其中x＝[δveδvnδlδλφxφyφz]，其中δve为东向速度误差、δvn为北向速度误差、δl为经度误差、δλ为纬度误差、φx、φy、φz分别为失准角，w和v为系统噪声和观测噪声，分别为零均值的高斯白噪声；
式中f和ha为：
根据建立的状态方程和量测方程，将状态方程和量测方程离散化之后，即能建立惯性导航/视觉的闭环卡尔曼滤波方程，将卡尔曼滤波方程得到的状态进行状态反馈后，即得到精确的采煤机姿态角，具体步骤为：
ⅰ、相关变量初始化，kalman滤波器初始化；
ⅱ、开始导航解算，对状态方程进行离散化处理，计算出系统状态转移矩阵；
ⅲ、到达卡尔曼滤波周期时，进行时间更新；
ⅳ、利用两个摄像头对激光标靶的十字激光标志点进行拍摄，对采煤机的姿态角进行实时求解；
ⅴ、利用惯性导航数据对采煤机的实时姿态、速度、位置进行求解；
ⅶ、将惯导求得的采煤机姿态角与利用双目视觉求得的采煤机姿态角作差值，其差值作为卡尔曼滤波器的量测输入；
ⅷ、求解最优估计状态向量
最终得出精确的采煤机姿态角。