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

权利要求书

1.一种煤矿巷道机器人定位与环境建模方法，其特征在于：它包括如下步骤，

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

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

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

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

其中：

状态变量x_k＝[▽_k,Δv_k,Δs_k,ΔK_k]，▽_k为加速度计随机常值零偏误差，Δ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为零均值白噪声，测量噪声方差阵为R_k，即E[ξ_k]＝0、

所述的步骤2)里程仪的误差包括刻度系数误差，以及由于打滑或滑行造成的随机误差，设机器人导航的全局坐标系为OXYZ为东北天坐标系，坐标原点为机器人导航的起始点，机器人相对坐标系OrXrYr定义为以机器人为原点，以机器人前进方向为Xr轴，垂直于Xr轴逆时针90°方向为Yr轴，在全局坐标系内，机器人航向角ψ定义为机器人Xr轴相对于X轴的夹角，偏北为正；俯仰角θ定义为Xr轴在OYZ平面内的投影与Y轴夹角，向上为正，在机器人相对坐标系内，目标航向角α定义为，目标与机器人连线相对于Xr轴的夹角，逆时针方向为正，

选用激光雷达探测的环境信息对里程仪误差进行修正，规定定位修正时激光雷达的俯仰角度为0，算法步骤如下：

(1)根据里程仪提供的位移增量和惯导系统提供的航向角ψ_k、俯仰角θ_k，从k-1时刻机器人在全局地图中的位置(x_k-1,y_k-1,z_k-1)，推算机器人在k时刻的预测位置

(2)根据已知的环境特征在全局地图中的位置(x_o,y_o,z_o)，以及机器人k时刻的预测位置计算环境特征在k时刻相对机器人的距离l_o：

(3)k时刻激光雷达对周围环境进行扫描，从扫描点中提取环境特征，得到当前关注的环境特征在机器人坐标系中的实测距离ρ_o和角度α_o；

(4)剔除异常数据：对比l_o与ρ_o，当l_o-ρ_o大于设定的打滑阈值M时，认为机器人车轮处于打滑状态；当l_o-ρ_o小于设定的滑行阈值N时，认为机器人车轮处于滑行状态；上述异常数据即不参与导航解算也不参与误差修正；

(5)根据由激光雷达获得的特征点在机器人坐标系的坐标(ρ_o,α_o)，以及环境特征在全局地图中的位置(x_o,y_o,z_o)，计算k时刻机器人的位置(x_k,y_k,z_k)，及机器人在k-1时刻到k时刻的实际里程d_k：

(6)计算里程仪刻度系数误差ΔK_k，在组合导航算法中用ΔK_k对里程仪测量值进行修正：

所述的步骤3)为实现对煤矿巷道的3D环境建模，选用二维激光雷达与高精度电控转台组成环境探测系统，二维激光雷达在电控转台的驱动下可以绕Yr轴转动，激光雷达的俯仰角β定义为向上为正，向下为负，

采用二维笛卡尔矩形栅格及高度图的方式来表示雷达探测到的环境，用一个二维数组T_m×n记录该环境地图：

根据k时刻机器人的位置(x_k,y_k,z_k)、航向角ψ_k、俯仰角θ_k，以及激光雷达俯仰角β_k和激光雷达探测到的障碍物信息(ρ_o,α_o)，可以计算障碍物在全局坐标系中的坐标(x_o,y_o,z_o)：

设栅格的大小为w×w，则障碍物占据的栅格的二维坐标(x_g,o,y_g,o)为：

int()表示取整运算

当二维激光雷达俯仰扫描时，