基于深度相机定位修正的采煤机组合导航定位系统及方法与流程

1.本发明涉及基于惯性导航系统的采煤机工作面检测，具体涉及一种基于深度相机定位修正的采煤机组合导航定位系统及方法。


背景技术：

[0002]“煤矿三机”指煤矿综采工作面的采煤机、刮板输送机及液压支架三种机械。采煤机和液压支架调高范围要适应煤层厚度及变化，液压支架移架要跟得上采煤机的牵引速度。采煤机依靠刮板输送机导向并在其上移动，刮板输送机依靠液压支架推移，液压支架又靠刮板输送机支承而移动。
[0003]
为了实现“煤矿三机”的联动,对采煤机的空间位置及姿态进行准确检测,即对采煤机进行空间动态定位具有重要意义。为了实现采煤机位置及姿态检测,有学者提出了采煤机惯性导航定位方法。捷联惯性导航系统是指将陀螺仪和加速度计直接固定在运载体上,利用陀螺仪和加速度计等惯性敏感器件对运行载体三轴角速度和三轴加速度信息进行实时测量,结合运行载体初始惯性信息,通过高速积分获得运动载体的姿态、速度及位置等导航信息。捷联惯性导航系统在工作时不依赖外界信息,也不向外界辐射能量,不易受到干扰破坏,是一种自主式导航系统,具有数据更新率高、数据全面以及短时定位精度高等优点。
[0004]
但同时,对于纯惯性导航系统也存在着一定的问题,由于其位置、姿态数据是通过基于陀螺和加速度计测量数据的多重积分解算而得，传感器的测量误差随时间不断累积，故其解算位姿数据误差会随时间不断发散，因此，长时间运行过程中往往需要对其累积误差进行修正。cn109931958a公开了一种基于uwb(ultra-wide-band)采煤机工作面端头校准装置及方法，在采煤机场景两侧巷道布置uwb基站，采煤机机身加装uwb接收机，当采煤机运行至工作面轨迹两端时通过接收巷道的uwb信号实现末端的精确定位，从而用以修正惯性导航系统的累积误差。该方法能有效通过末端uwb高精度定位辅助修正惯导定位精度，但该方法设置过程中需要将巷道两端的uwb基站不断移动并通过巷道外的全站仪对uwb基站进行精确标定，操作较为繁琐，并且该方案流程中在末端通过uwb校准后未进行全局优化，最终输出的轨迹数据易出现跳点且精度并未达到全局最优；cn108731672a公开了一种基于双目视觉及惯导的采煤机姿态检测系统和方法，该方法在采煤机体上安装双目摄像头和激光靶标，远处激光指向仪发射十字激光照在激光标靶上，通过利用两个摄像头对激光标靶进行实时拍摄跟踪，基于双目视觉原理进行立体匹配得到标志点三维坐标从而解算出采煤机的实时姿态，并通过卡尔曼滤波与惯性导航解算数据实时融合。该方法通过视觉解算的姿态数据实时修正惯性导航，能一定程度上提升最终定位精度，但仅仅修正惯导姿态，最终组合输出的位置理论上还是处于误差不断发散状态，且煤矿环境粉层较大，视觉传感器不适合在中间过程长期使用。cn111207742a公开了一种附加外方位元素约束的采煤机定位定姿方法， cn108106569a公开了一种基于视觉的综合工作面直线度检测方法和系统，以上几种方案都要求视觉传感器在采煤机运作中间过程中一直工作，而视觉传感器并不适合在煤矿
灰尘环境下长时间工作。


技术实现要素：

[0005]
针对单纯依赖惯性导航系统的采煤机位姿检测误差随时间不断累积问题，本发明提供基于深度相机定位修正的采煤机组合导航定位系统及方法，通过深度相机在工作面轨迹末端对指定视觉特征板进行识别、匹配及位姿解算得到采煤机末端位姿数据，再基于全局优化算法与惯导系统输出位姿数据进行有效融合，最终得到经过末端修正优化后的整个工作面轨迹位姿数据。
[0006]
一种基于深度相机定位修正的采煤机组合导航定位系统，包括惯性导航系统、深度相机、轮速里程计、以及布置于采煤机运行轨迹两端的视觉特征标记板；惯性导航系统安装于采煤机机身中间位置；采煤机头尾两端各安装一个深度相机，以便采煤机在来回运行的末端位置处，能通过深度相机识别轨迹两端位于巷道的视觉特征标记板；视觉特征标记板在巷道每间隔一定距离摆放一块，间隔距离取决于深度相机视场，需确保随着采煤工作面往前推移，在轨迹两端深度相机视场内都能观测到视觉特征标记板，同时两边巷道的视觉特征标记板在空间上一一对应，对应的两块视觉特征标记板为一组；轮速里程计安装在采煤机滚轮轴承位置；轮速里程计数据和惯性导航系统数据、深度相机识别匹配位姿数据发送至机载计算机，在计算机内完成最终多传感器组合导航定位解算。
[0007]
所述采煤机组合导航定位系统的采煤机组合导航定位方法，步骤包括：步骤1）根据深度相机视场范围、巷道的宽度及视觉特征标记板的尺寸，按一定间隔距离在巷道侧壁间隔布置固定视觉特征标记板，同时确保两边巷道的视觉特征板在空间位置上沿着直线工作面轨迹方向能够一一对应；步骤2）首先采煤机在工作面轨迹起始点处静止，深度相机对标记板进行相关特征识别及坐标计算，同时惯性导航系统进行初始对准解算，结合惯导初始对准输出的位姿将相机坐标系下标记板特征坐标转换至东北天参考坐标系下，完成起点处视觉特征标记板的三维空间坐标标定；然后，采煤机以空载最快速度运行至工作面轨迹另一端并通过惯导和轮速实时积分解算出终点处的采煤机位姿，采煤机于终点处静止后深度相机对巷道内对应的标记板进行相关特征识别及坐标计算，再结合惯性导航系统输出的位姿将相机坐标系下标记板特征坐标转换至东北天参考坐标系下，完成终点处标记板的三维空间坐标标定，到此，一组视觉特征标记板的空间坐标完成标定；巷道内对应一组视觉特征标记板完成三维空间坐标标定后，便可用于末端位姿修正，接下来步骤如下：步骤3）采煤机在截割运动过程中，惯性导航系统和轮速里程计进行实时融合定位解算，保存实时解算的位姿及对应协方差数据；步骤4）当采煤机运行至工作面轨迹末端时，启动深度相机识别定位系统，识别解算标记板在相机坐标系下的特征点位置，结合标记板特征点在东北天参考坐标系下的位置，可匹配解算出采煤机在东北天参考坐标系下的位姿；步骤5）将惯性导航和里程计保存的位姿数据、协方差数据与通过深度相机识别解算的位姿数据，通过离线优化算法进行全局最优平滑解算，得到末端修正优化后的工作面
轨迹解算值。
[0008]
所述的采煤机组合导航定位方法，步骤6）随着采煤机来回切割，工作面轨迹沿着煤壁方向不断推移，一旦导致采煤机两端深度相机无法识别到本组视觉特征标记板时，在巷道上启用下一组视觉特征标记板，通过空间坐标标定，然后采煤机在末端通过深度相机识别修正惯性导航系统输出轨迹，重复2）~5）步骤；若来回切割本组视觉特征标记板仍在深度相机视场范围内，则重复步骤3）~5）。
[0009]
所述的深度相机包括双目立体视觉相机、结构光深度相机和tof深度相机。
[0010]
所述的采煤机组合导航定位方法，仅在工作面轨迹末端进行深度相机定位解算，在中间运行过程不开启。
[0011]
本发明的有益效果及优点：基于深度相机末端定位修正的采煤机组合导航定位方法在采煤机机头机尾分别设置深度相机，通过在工作面轨迹末端识别匹配视觉特征标记板解算得采煤机在末端的精确位姿，并与惯性导航解算的轨迹数据进行全局优化融合，能有效修正惯导航系统随时间累积的误差，提升全局工作面轨迹位姿精度；本方法只在工作面轨迹的末端进行视觉定位解算及修正，深度相机镜头在轨迹中间运行过程中可以通过遮蔽设备进行遮蔽保护，有效屏蔽煤矿灰层对镜头的影响；另外在本发明中，除了需要在巷道布置视觉特征标记板，并不需要额外高成本设备在现场辅助，例如全站仪、激光仪等。
附图说明
[0012]
图1为基于深度相机定位修正的采煤机组合导航定位系统的结构示意图。
[0013]
图2为惯性导航系统、深度相机、轮速里程计在采煤机上安装示意图。
[0014]
图3为安装于巷道的视觉特征标记板空间坐标标定流程。
[0015]
图4为基于深度相机末端定位修正的惯性/轮速/视觉多传感器组合导航方案流程图。
[0016]
图中，1为采煤机机身，2为惯性导航单元，3为安装于采煤机前后两侧的深度相机，4为视觉特征标记板，5为轮速里程计。
具体实施方式
[0017]
为了使本发明的目的、技术方案及优点更加清楚，以下根据结合附图和实施例对本发明做进一步的阐述。
[0018]
如图1、2所示，一种基于深度相机定位修正的采煤机组合导航定位系统，主要包括惯性导航系统2、深度相机3、轮速里程计5、以及布置于采煤机运行轨迹两端用于视觉特征标记板4。惯性导航系统2安装于采煤机机身1中间位置；采煤机头尾两端各安装一个深度相机3，以便采煤机在工作面轨迹来回运行到末端位置处时，能通过深度相机3识别轨迹两端位于巷道的视觉特征标记板4；视觉特征标记板4在巷道每间隔一定距离摆放一块，标记板4摆放的间隔距离取决于深度相机3视场，需确保随着采煤工作面往前推进，在轨迹两端深度相机3视场内都能观测到标记板4，同时两边巷道的标记板4在空间上需一一对应，对应的两块标记板4为一组，如图1所示空间上沿着工作面轨迹正对的两块标记板即为对应的一组标记板；采煤机滚轮轴承位置安装有轮速里程计5，轮速里程计5数据和惯性导航系统2数据、深度相机3识别匹配位姿数据通过数据微处理单元与计算机相连，在计算机内完成最终多
传感器组合导航定位解算。
[0019]
如图3所示，一种基于深度相机定位修正的采煤机组合导航定位方法，步骤包括：步骤（1）根据深度相机3视场范围、巷道的宽度及视觉特征标记板的尺寸，按一定间隔距离在巷道侧壁间隔布置固定视觉特征标记板4，同时确保两边巷道的视觉特征板4在空间位置上沿着直线工作面轨迹方向能够一一对应，视觉特征标记板在巷道中的布局概况如图1所示；步骤（2）在使用深度相机3进行末端位姿修正之前需首先对视觉特征标记板4进行三位空间坐标标定，具体标定流程如图3所示，采煤机在工作面轨迹起始点处，深度相机3对标记板4进行相关特征识别及相机坐标系下坐标计算，同时惯性导航系统2进行初始对准，结合惯导初始对准输出的位姿将相机坐标系下标记板4特征坐标转换至东北天参考坐标系下，这样便完成起点处标记板4的三位空间坐标标定；采煤机以空载最快速度运行至工作面轨迹另一端并通过惯导和轮速实时积分解算出终点处的采煤机位姿，采煤机于终点处静止后深度相机3对巷道内对应的标记板4进行相关特征识别及坐标计算，再结合惯导积分输出的位姿将相机坐标系下标记板特征坐标转换至东北天参考坐标系下，这样便完成终点处标记板4的三维空间坐标标定，这样便完成一组视觉特诊标记板的空间坐标标定；巷道内对应一组视觉特征标记板4完成空间坐标标定后，便可用于末端位姿修正，接下来步骤如下：步骤（3）采煤机在截割运动过程中，惯性导航单元2和轮速里程计5通过卡尔曼滤波进行实时定位解算，保存实时解算的位姿及对应协方差等数据；步骤（4）当采煤机运行至工作面轨迹末端时，启动深度相机3识别定位系统，识别解算标记板4在相机坐标系下的特征点位置，结合标记板特征点在东北天参考坐标系下的位置，可匹配解算出采煤机在东北天参考坐标系下的位姿；步骤（5）将惯性导航2和里程计5保存的位姿数据、协方差数据与通过深度相机3识别解算的位姿数据，通过离线优化算法进行全局最优平滑解算，可得到末端修正优化后的工作面轨迹解算值，通过全局优化算法可确保修正后的轨迹无跳点且精度最优；步骤（7）随着采煤机来回切割，工作面轨迹沿着煤壁方向不断推移，一旦导致采煤机两端深度相机无法识别到本组视觉特征标记板时，需在巷道上启用下一组视觉特征标记板，通过空间坐标标定，然后采煤机可在末端通过深度相机识别修正惯导输出轨迹，重复（2）~（5）步骤；若来回切割本组视觉特征标记板仍在深度相机视场范围内，则重复步骤（3）~（5）；本方法采用的深度相机可包括双目立体视觉相机、结构光深度相机和tof深度相机，视觉特征标记板适用于对应的深度相机特征提取及识别即可。
[0020]
本方法仅在工作面轨迹末端进行深度相机定位解算，在中间运行过程不需要开启，故可对深度相机镜头加装保护遮盖防止中间过程煤矿粉尘影响，当采煤机运行在工作面轨迹两侧时镜头遮盖打开即可。
[0021]
如图4所示，在本方法中，惯性导航系统2基于imu传感器输出的角速率、加速度数据完成三轴姿态、速度、位置的捷联解算，同时imu传感器输出角速度数据与轮速里程计5输出轮速数据联合完成航位推算，将航位推算结果与捷联解算结果通过ekf算法融合输出惯导/轮速实时融合定位结果，保存整条轨迹实时融合定位的结果包括位姿数据及对应的协
方差数据。当采煤机运行至轨迹末端时，深度相机3通过图像预处理、特征识别等过程完成末端视觉特征标记板4的识别及位姿匹配解算，然后将深度相机末端匹配解算的位姿数据与保存的惯导/轮速融合定位的整条轨迹位姿数据通过离线全局优化算法到末端修正优化后的工作面轨迹解算值。
[0022]
以上所述仅为本发明的较佳的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到的变化或者替换，都应涵盖在本发明保护范围之内。