[发明专利]一种煤矿巷道机器人定位与环境建模方法

说明书

本发明属于一种适用于地面移动机器人在煤矿地下巷道环境中自主定位和环境建模的方法。它包括如下步骤，1)机器人组合定位算法；2)基于环境信息的定位修正；3)机器人3D环境建模方法。本发明的优点是，通过煤矿巷道测量移动机器人能够自动生成巷道的三维模型，可以立体、直观、准确地表现并反映井下巷道及其空间关系，对于指导现场生产、培训矿工安全生产有着积极意义。

技术领域

本发明属于一种智能移动机器人导航控制方法，具体涉及一种适用于地面移动机器人在煤矿地下巷道环境中自主定位和环境建模的方法。

背景技术

智能移动机器人是一类能够通过传感器感知环境和自身状态，实现在有障碍物的环境中面向目标的自主导航运动，从而完成预定任务的机器人系统。要实现机器人自主导航运动，必须要解决轨迹规划、运动控制、环境建模、实时定位等一系列问题。

煤矿巷道的三维模型，可以立体、准确地反映井下巷道及其空间关系，对于指导现场生产、培训矿工安全生产、实施井下救援有着积极意义，是矿山数字化建设的重要内容之一，为实现矿山井下环境的可视化虚拟再现奠定了基础。

煤矿地下巷道环境复杂，尤其在当煤矿发生坍塌、瓦斯爆炸等事故时，井下条件十分恶劣，给测量工作带来很大困难，甚至危及测量人员的安全。将智能机器人技术应用于煤矿巷道探测，将为煤矿井下测绘提供一种安全、快速、有效的解决途径。

发明内容

本发明的目的是提供一种煤矿巷道机器人定位与环境建模方法，它能够用于煤矿地下巷道探测的机器人定位、环境建模算法。同步定位与建图算法模块，通过融合多种传感器信息，在精确定位的同时，建立井下巷道的3D环境模型。

本发明是这样实现的，一种煤矿巷道机器人定位与环境建模方法，它包括如下步骤，

1)机器人组合定位算法；

2)基于环境信息的定位修正；

3)机器人3D环境建模方法。

所述的步骤1)为在煤矿井下环境中，选用基于Kalman滤波的中精度惯导系统+里程仪的组合导航定位方式，中等精度惯导系统具有较强的姿态保持能力，但定位误差会随时间不断累积；而里程仪的测量误差一般随里程增加，两者具有很强的互补性，通过组合可以构成高精度的自主导航系统，

惯导/里程仪组合导航系统的状态方程为：x_k＝A_kx_k-1+w_k-1，

其中：

状态变量为加速度计随机常值零偏误差，Δv_k、Δs_k分别为惯性导航解算的速度误差和位置误差，ΔK_k为里程仪刻度系数误差；

状态转移矩阵T为采样时间；

系统噪声W_k为零均值白噪声，噪声方差阵为Q_k，即E[w_k]＝0、

惯导/里程仪组合导航系统的测量方程为：z_k＝H_kx_k+ξ_k，

其中：观测量Z_k为从t_k-1时刻到t_k时刻，惯导解算的位移增量与里程仪测量的位移增量之差，即在剔除打滑和滑行等随机误差后，里程仪测量值与真实值d_k之间的关系为

测量矩阵

测量噪声ξ_k为零均值白噪声，测量噪声方差阵为Rk，即E[ξ_k]＝0、