[发明专利]一种基于UWB与惯性单元的地下巷道掘进装备实时定位方法

权利要求书

1.一种基于UWB与惯性单元的地下巷道掘进装备实时定位方法，采用了一种定位装置，所述定位装置包括UWB雷达发射机、UWB雷达接收机和惯性测量单元；UWB雷达发射机安装在巷道内，UWB雷达接收机、惯性测量单元安装在掘进设备上；其特征在于，定位方法包括以下步骤：

步骤一：测量UWB雷达发射机、UWB雷达接收机在地理坐标系下的坐标；测量惯性测量单元相对于掘进设备的位置关系；测量UWB雷达相对于惯性测量单元的位置关系；求得掘进设备在移动过程中在地理坐标系下的坐标(X₀,Y₀,Z₀)，以及初始姿态角度为(β₀,α₀,γ₀)，其中，α₀为俯仰角、β₀为横滚角、γ₀为偏航角；

步骤二：通过惯性测量单元得到掘进设备在X、Y、Z三个方向的位移增量ΔX、ΔY、ΔZ，以及围绕着X、Y、Z三个轴的角度偏移量Δβ、Δα、Δγ；进一步得到掘进设备在行驶过程中的实时位置(X₀+ΔX,Y₀+ΔY,Z₀+ΔZ)，以及实时姿态角度为(β₀+Δβ,α₀+Δα,γ₀+Δγ)；

步骤三：结合UWB雷达测距的方式观察掘进设备位置，得到观测值；通过惯性测量单元得到掘进设备的动作值，采用卡尔曼滤波的方式对观测值和动作值进行处理，修正定位误差。

2.根据权利要求1所述的一种基于UWB与惯性单元的地下巷道掘进装备实时定位方法，其特征在于，所述步骤一UWB雷达发射机、UWB雷达接收机在地理坐标系下的坐标，惯性测量单元相对于掘进设备的位置关系，UWB雷达相对于惯性测量单元的位置关系均通过全站仪或人工放线的方式测量得到。

3.根据权利要求2所述的一种基于UWB与惯性单元的地下巷道掘进装备实时定位方法，其特征在于，所述惯性测量单元配备的是自标定的单轴激光陀螺仪。

4.根据权利要求3所述的一种基于UWB与惯性单元的地下巷道掘进装备实时定位方法，其特征在于，所述惯性测量单元内含有三个单轴加速度计；三个单轴加速度计提供掘进装备在行驶工作过程中在X、Y、Z轴三个相交垂直方向上实时的加速度a_x、a_y、a_z；对加速度对时间的积分得到各个方向上的速度分量V_x、V_y、V_z，再次对时间进行积分得到各个方向上的位移ΔX、ΔY、ΔZ。

5.根据权利要求4所述的一种基于UWB与惯性单元的地下巷道掘进装备实时定位方法，其特征在于，所述惯性测量单元内含三个单轴激光陀螺仪；三个单轴激光陀螺仪分别测得围绕着X、Y、Z三个轴的角加速度α_β、α_α、α_γ，通过对时间的积分得到围绕着X、Y、Z三个轴的角速度ω_β、ω_α、ω_γ，进一步对角速度对时间积分，得到掘进设备围绕着X、Y、Z三个轴的角度偏移量Δβ、Δα、Δγ。

6.根据权利要求3～5任意一项所述的一种基于UWB与惯性单元的地下巷道掘进装备实时定位方法，其特征在于，惯性测量单元测量到的数据中，横滚角度Δβ、俯仰角度Δα通过重力实时修正观测，偏航角度Δγ通过惯性测量单元自身配备的自标定功能来定期归零；纵向偏移量和竖向偏移量产生的误差受到掘进设备机身轮胎的限制，满足精准定位的精度；横向偏移误差没有限制也不能自动修正，会随着时间成二次方的增长，需要对横向偏移量进行修正误差。