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

摘要

本发明公开了巷道掘进机的实时连续自主定位定向系统，三维激光扫描雷达固定安装在捷联惯导系统顶部，标记板安装在三维激光扫描雷达的侧面，标记板三维激光扫描雷达在同一高度，二者的相对位置固定不变；捷联惯导系统固定安装在底板上；激光雷达处理器与三维激光扫描雷达连接；捷联惯导系统处理器与捷联惯导系统连接；总处理器分别与捷联惯导系统处理器及激光雷达处理器连接；存储通信模块与总处理器连接。本发明无需另外设置无线通信定位基站，一旦正常启动，不再需要人为参与，便可实现掘进机的自主导航定位。

主权项

1.巷道掘进机的实时连续自主定位定向系统，其特征在于，包括定位装置、标记板及客户端，所述定位装置包括底板、捷联惯导系统、三维激光扫描雷达、激光雷达处理器、捷联惯导系统处理器、总处理器、掘进机控制器及存储通信模块，定位装置通过底板安装在掘进机的后方，定位装置用于测量掘进机已挖掘部分的空间位置，其中：/n三维激光扫描雷达固定安装在捷联惯导系统顶部，标记板安装在三维激光扫描雷达的侧面，标记板三维激光扫描雷达在同一高度，二者的相对位置固定不变，标记板用于作为三维激光扫描雷达测量时的参考标记，三维激光扫描雷达能够同时采集巷道点云信息及标记板点云信息；/n捷联惯导系统固定安装在底板上，捷联惯导系统用于采集掘进机的加速度及角速度信息；/n激光雷达处理器与三维激光扫描雷达连接，激光雷达处理器用于将三维激光扫描雷达得到的巷道点云信息和标记板点云信息进行滤波并提取点云几何特征；/n捷联惯导系统处理器与捷联惯导系统连接，捷联惯导系统处理器用于将捷联惯导系统采集的加速度及角速度信息通过滤波和姿态更新计算得到第一姿态变换矩阵；/n总处理器分别与捷联惯导系统处理器及激光雷达处理器连接，总处理器用于基于所述捷联惯导系统处理器得到的第一姿态变换矩阵和所述激光雷达处理器得到的点云几何特征数据生成巷道的三维地图，计算出当前时刻掘进机在三维地图上的位置和姿态信息，以及与预先设定轨迹的偏差和当前时刻的修正量；/n所述基于所述捷联惯导系统处理器得到的第一姿态变换矩阵和所述激光雷达处理器得到的点云几何特征数据生成巷道的三维地图，计算出当前时刻掘进机在三维地图上的位置和姿态信息，以及与预先设定轨迹的偏差和当前时刻的修正量包括：当掘进机停止挖掘时，捷联惯导系统处理器将所述捷联惯导系统得到的加速度及角速度信息通过滤波、姿态更新计算得到第一姿态变换矩阵，根据所述激光雷达处理器得到的点云几何特征，运用迭代最近点算法，得到前后两帧点云数据之间的第二姿态变换矩阵；根据所述捷联惯导系统处理器得到的第一姿态变换矩阵进行加权平均，得到第三姿态变换矩阵：