[发明专利]一种井下掘进机定位导向方法及井下掘进机定位导向系统

权利要求书

1.一种井下掘进机定位导向方法，其特征在于，所述方法包括：

S1，根据井下通视情况确定多个基准点，在每个基准点同时设置基准棱镜和无线电基站；

S2，利用全站仪通过所述基准棱镜进行自主定位，并且在完成自主定位后通过搜索追踪机载棱镜获取所述机载棱镜的实时位置信息；

S3，利用机载超宽带定位装置通过所述无线电基站进行定位解算以获取所述机载超宽带定位装置的实时位置信息；

S4，利用机载惯导装置根据所述机载棱镜的实时位置信息和所述机载超宽带定位装置的实时位置信息对所述掘进机进行组合导航以输出实时组合导航结果。

2.根据权利要求1所述的方法，其特征在于，所述方法还包括：所述机载惯导装置根据掘进机控制器的起停信号判断所述掘进机的运行状态，并且在所述运行状态为静止时利用零速进行自主修正。

3.根据权利要求2所述的方法，其特征在于，S4包括：

S41，利用所述机载惯导装置对所述掘进机进行单独导航解算以获取原始实时导航结果；

S42，建立基于机载惯导装置和机载全站仪的第一子滤波器、基于机载惯导装置和机载超宽带定位装置的第二子滤波器以及基于机载惯导装置和两个子滤波器的主滤波器；

S43，基于自适应分配比重算法利用所述第一子滤波器、所述第二子滤波器和所述主滤波器根据所述原始实时导航结果、所述机载棱镜的实时位置信息以及所述机载超宽带定位装置的实时位置信息进行联邦卡尔曼滤波以实时获取所述机载惯导装置的综合误差估计值；

S45，根据所述综合误差估计值对所述原始实时导航结果进行修正以得到修正后的实时导航结果，并将修正后的实时导航结果作为所述实时组合导航结果进行输出。

4.根据权利要求3所述的方法，其特征在于，S43包括：

利用所述第一子滤波器对所述原始实时导航结果以及所述机载棱镜的实时位置信息进行卡尔曼滤波以实时获取所述机载惯导装置的第一误差估计值和所述第一误差估计值的协方差；

利用所述第二子滤波器对所述原始实时导航结果以及所述机载超宽带定位装置的实时位置信息进行卡尔曼滤波以实时获取所述机载惯导装置的第二误差估计值和所述第二误差估计值的协方差；

利用所述主滤波器基于所述自适应分配比重算法根据所述原始实时导航结果、所述第一误差估计值、所述第一误差估计值的协方差、所述第二误差估计值以及所述第二误差估计值的协方差进行卡尔曼滤波以实时获取所述机载惯导装置的第三误差估计值和所述第三误差估计值的协方差；

将所述第三误差估计值的协方差与预设阈值进行比较，并在所述第三误差估计值的协方差小于所述预设阈值时将对应的第三估计误差值作为综合误差估计值。

5.根据权利要求4所述的方法，其特征在于，所述实时组合导航结果包括实时组合导航位置信息、实时组合导航姿态信息和实时组合导航速度信息。

6.根据权利要求5所述的方法，其特征在于，所述原始实时导航结果包括原始实时导航位置信息、原始实时导航姿态信息和原始实时导航速度信息。

7.根据权利要求6所述的方法，其特征在于，所述综合误差估计值包括综合姿态角误差估计值、综合速度误差估计值和综合位置误差估计值。

8.根据权利要求7所述的方法，其特征在于，所述第一子滤波器的观测量为所述第二子滤波器的观测量为其中，Z_ETS表示第一子滤波器的观测量，φ表示机载惯导装置单独导航解算的纬度，h表示机载惯导装置单独导航解算的高度，λ表示机载惯导装置单独导航解算的经度，φ_ETS表示机载棱镜的纬度，h_ETS表示机载棱镜的高度，λ_ETS表示机载棱镜的经度，Z_UWB表示第二子滤波器的观测量，φ_UWB表示机载超宽带定位装置的纬度，h_UWB表示机载超宽带定位装置的高度，λ_UWB表示机载超宽带定位装置的经度。