[发明专利]一种掘进机的导向方法、系统和存储介质

权利要求书

1.一种掘进机的导向系统，其特征在于，包括：

测量装置，包括陀螺仪和里程计；

所述陀螺仪安装于防爆控制盒内，所述防爆控制盒安装于掘进机的顶部；

所述陀螺仪，用于获取所述掘进机的姿态角；所述姿态角包括俯仰角、滚转角、方位角；

所述里程计，安装于所述掘进机的行走部，用于获取输出脉冲数；

处理器，分别与所述陀螺仪和里程计连接，用于根据所述输出脉冲数、姿态角以及预先获取的初始标定关系矩阵，计算得到所述掘进机的机头中心点、机尾中心点在导航坐标系下的位置坐标，再根据所述位置坐标和计划线计算得到掘进机的姿态偏差；

所述处理器包括：

标定模块，用于获取在初始状态从全站仪获取所述掘进机的机头中心点、机尾中心点分别在导航坐标系下的刚体坐标，并获取在所述初始状态下所述掘进机的初始标定关系矩阵；

其中，所述初始标定关系矩阵为载体坐标系在所述初始状态下相对于掘进机坐标系的矩阵，代表载体坐标系与掘进机坐标系的刚体关系；

所述标定模块包括：

第一获取子模块，用于在初始状态利用全站仪获取机尾中心点、机头中心点分别在导航坐标系下的初始坐标，并从所述陀螺仪处获取初始姿态角；所述初始姿态角包括初始俯仰角、初始横滚角和初始航向角；

第一计算子模块，用于根据所述初始坐标、计划线计算得到初始旋转矩阵，并根据所述初始姿态角计算得到零位旋转矩阵，再根据所述初始旋转矩阵和零位旋转矩阵计算得到所述初始标定关系矩阵；

；

其中，ls为计划线对应的第一直线方向矢量，lt为利用机尾中心点A、机头中心点B分别在导航坐标系下的初始坐标和计算得到导航坐标系下的第二直线方向矢量，为零位旋转矩阵，为陀螺仪在初始状态获取到的在载体坐标系下的初始俯仰角，为陀螺仪在初始状态获取到的在载体坐标系下的初始横滚角，为陀螺仪在初始状态获取到的在载体坐标系下的初始航向角，为载体坐标系到掘进机坐标系的初始标定关系矩阵，为零位旋转矩阵的逆矩阵，为初始状态下在导航坐标系下的初始旋转矩阵。

2.根据权利要求1所述的掘进机的导向系统，其特征在于，所述处理器还包括：

初始对准模块，用于根据从所述陀螺仪处获取的姿态角，计算得到导航坐标系相对于载体坐标系的初始姿态转换矩阵；

导向规划模块，用于根据所述掘进机在导航坐标系下的初始空间坐标和预先设定的目标空间坐标，拟合生成所述掘进机在所述导航坐标系下的计划线；

实时测量模块，用于获取所述陀螺仪测量得到的实时姿态角，并获取所述里程计测量得到的输出脉冲数；所述实时姿态角包括实时航向角、实时横滚角、实时俯仰角；

里程计算模块，用于根据所述输出脉冲数和所述里程计的出厂参数，计算得到所述掘进机的里程增量；

实时对准模块，用于根据实时姿态角计算得到所述导航坐标系相对于所述载体坐标系的实时姿态转换矩阵；

偏差计算模块，用于根据所述初始标定关系矩阵、实时姿态转换矩阵、里程增量，以及所述陀螺仪在所述导航坐标系下的刚体坐标计算得到所述位置坐标，再根据所述位置坐标和计划线计算得到掘进机的姿态偏差；

其中，为初始姿态转换矩阵，为初始姿态转换矩阵的逆矩阵，为导航坐标系相对于载体坐标系的姿态转换矩阵，为姿态转换矩阵的转置矩阵，为载体坐标系相对于导航坐标系的航向角，为载体坐标系相对于导航坐标系的俯仰角，为载体坐标系相对于导航坐标系的横滚角，是陀螺仪于当前采样周期t在载体坐标系下的实时俯仰角，是陀螺仪于当前采样周期t在载体坐标系下的实时横滚角，是陀螺仪于当前采样周期t在载体坐标系下的实时航向角，为导航坐标系相对于载体坐标系的实时姿态转换矩阵。