[发明专利]巷道掘进机的实时连续自主定位定向系统及导航定位方法

权利要求书

1.巷道掘进机的实时连续自主定位定向系统，其特征在于，包括定位装置、标记板及客户端，所述定位装置包括底板、捷联惯导系统、三维激光扫描雷达、激光雷达处理器、捷联惯导系统处理器、总处理器、掘进机控制器及存储通信模块，定位装置通过底板安装在掘进机的后方，定位装置用于测量掘进机已挖掘部分的空间位置，其中：

三维激光扫描雷达固定安装在捷联惯导系统顶部，标记板安装在三维激光扫描雷达的侧面，标记板三维激光扫描雷达在同一高度，二者的相对位置固定不变，标记板用于作为三维激光扫描雷达测量时的参考标记，三维激光扫描雷达能够同时采集巷道点云信息及标记板点云信息；

捷联惯导系统固定安装在底板上，捷联惯导系统用于采集掘进机的加速度及角速度信息；

激光雷达处理器与三维激光扫描雷达连接，激光雷达处理器用于将三维激光扫描雷达得到的巷道点云信息和标记板点云信息进行滤波并提取点云几何特征；

捷联惯导系统处理器与捷联惯导系统连接，捷联惯导系统处理器用于将捷联惯导系统采集的加速度及角速度信息通过滤波和姿态更新计算得到第一姿态变换矩阵；

总处理器分别与捷联惯导系统处理器及激光雷达处理器连接，总处理器用于基于所述捷联惯导系统处理器得到的第一姿态变换矩阵和所述激光雷达处理器得到的点云几何特征数据生成巷道的三维地图，计算出当前时刻掘进机在三维地图上的位置和姿态信息，以及与预先设定轨迹的偏差和当前时刻的修正量；

所述基于所述捷联惯导系统处理器得到的第一姿态变换矩阵和所述激光雷达处理器得到的点云几何特征数据生成巷道的三维地图，计算出当前时刻掘进机在三维地图上的位置和姿态信息，以及与预先设定轨迹的偏差和当前时刻的修正量包括：当掘进机停止挖掘时，捷联惯导系统处理器将所述捷联惯导系统得到的加速度及角速度信息通过滤波、姿态更新计算得到第一姿态变换矩阵，根据所述激光雷达处理器得到的点云几何特征，运用迭代最近点算法，得到前后两帧点云数据之间的第二姿态变换矩阵；根据所述捷联惯导系统处理器得到的第一姿态变换矩阵进行加权平均，得到第三姿态变换矩阵：其中，是最终的掘进机坐标系到导航坐标系的第三姿态变换矩阵，是由捷联惯导系统得到的掘进机坐标系到导航坐标系的第一姿态变换矩阵，是由雷达点云数据得到的掘进机坐标系到导航坐标系的第二姿态变换矩阵，k₁是第一姿态变换矩阵的权重，k₂是第二姿态变换矩阵的权重；将得到的点云几何特征与第三姿态变换矩阵相乘，实现巷道地图的三维重建；进而得到掘进机在导航坐标系下的历史路径，结合预设的路径信息，便可得到当前时刻的偏差量和修正量；

掘进机控制器与总处理器连接，掘进机控制器用于基于所述总处理器给出的偏差和修正量，控制掘进机调整当前的姿态；

存储通信模块与总处理器连接，存储通信模块用于存储掘进机在三维地图上的位置和姿态信息，并将掘进机在三维地图上的位置和姿态信息发送至客户端。

2.如权利要求1所述的巷道掘进机的实时连续自主定位定向系统，其特征在于，定位装置还包括防尘罩及伺服控制电机，防尘罩安装在底板上，伺服控制电机能够响应总处理器的控制信号，控制防尘罩打开或闭合。

3.如权利要求1所述的巷道掘进机的实时连续自主定位定向系统，其特征在于，底板均布设置有多个缓震胶垫。

4.如权利要求1所述的巷道掘进机的实时连续自主定位定向系统，其特征在于，客户端通过无线网络与总处理器相连。

5.如权利要求1所述的巷道掘进机的实时连续自主定位定向系统，其特征在于，还包括巷道内的参考标记智能小车，参考标记智能小车跟随在掘进机之后，在掘进机移动以及三维激光扫描雷达开始测量的时候，参考标记智能小车不能移动，当掘进机与参考标记智能小车的距离超出激光扫描雷达的测量范围时，总处理器控制参考标记智能小车向前移动，此时掘进机不能移动，当参考标记智能小车到达预定地点，并由三维激光扫描雷达重新确定位置后，掘进机可再次移动挖掘，参考标记智能小车具有能够被三维激光扫描雷达识别的形状特征，将参考标记智能小车放置在巷道内，作为巷道固有特征的一部分，以提高巷道点云配准的效率和精度，进而提高掘进机导航的精度。