掘进机位姿测量与控制方法及系统

本发明涉及一种掘进机位姿测量与控制方法及系统。所述方法使用的系统包括巷道内的基站端测量系统、掘进机机身上的移动端测量系统和运动控制模块，所述方法包括以下步骤：S1，建立基站端坐标系；S2，建立移动端坐标系；S3，基站端通过视觉传感器识别测量移动端特征光源的矢量信息；S4，基站端和移动端通过通信测距设备测量两者之间的距离；S5，解算移动端在基站端坐标系下三维位置；S6，移动端通过视觉传感器识别测量基站端激光束的矢量信息；S7，基于EKF算法进行组合导航解算；S8，基于组合导航解算结果进行位姿控制。本发明采用双向视觉测量、通信测距与惯性测量相结合的手段，提高了测量精度，且不需要高成本的惯性测量单元。