一种基于超宽带和惯性单元融合的井下移动装备定位方法技术

本发明专利技术公开了一种基于超宽带和惯性单元融合的井下移动装备定位方法，首先对六路惯性单元数据进行融合并解算：通过姿态变换矩阵使得六路惯性单元的三轴角速度和三轴比例加速度均转化为钻机坐标系下的参数，并进行数据融合得到融合比例加速度、融合角速度，解算获得钻机的位姿信息；再次将惯性单元获得的融合加速度与融合角速度、以及超宽带的三维位置融入因子图，建立IMU/UWB的因子图模型，并获得最小误差函数；最后通过哈里斯鹰优化算法寻找最优值、进而获得钻机三维坐标；本发明专利技术不仅避免单一UWB定位造成定位信息跳变，有效收束UWB的定位误差，并避免单一IMU定位造成累积误差，而且可以灵活组合不同量测频率的传感器，实现即插即用的功能。即用的功能。即用的功能。

【技术实现步骤摘要】
一种基于超宽带和惯性单元融合的井下移动装备定位方法


[0001]本专利技术涉及无人定位技术，属于煤矿井下领域，具体涉及一种基于超宽带和惯性单元融合的井下移动装备定位方法。

技术介绍

[0002]冲击地压是指在煤炭开采过程中，巷道开挖或者工作面的煤体采出，将引起煤岩体应力的重新分布，当煤体间的应力达到极限时，煤体、顶板和底板中的能量释放形成应力波，导致岩石爆裂并弹射出来的现象。为防止冲击地压造成井巷的破坏甚至人员伤亡，最有效的防治方法是在巷道或者工作面上进行钻孔卸压；
[0003]现有钻孔设备以及相应系统越来越无人化、智能化，能够有效解决复杂条件下巷道掘进高危险性、低效率的问题，比如防冲钻孔机器人，其具备移动、钻孔等系列动作，能够对巷道或者工作面上进行钻孔卸压；
[0004]无人定位是防冲钻孔机器人进行系列动作的必要前提、也是钻孔卸压效果好坏的重要因素；目前防冲钻孔机器人的无人定位主要包括超宽带(简称UWB)定位、以及惯性单元(简称IMU)定位；UWB定位是通过超带宽无线通信进行定位，即利用发送和接收的传输时间计算出两设备的距离，并结合角度信息进行几何定位方法求得待定位目标的位置信息，但其在定位过程中会出现定位信息跳变，缺少过程信息等缺点；IMU定位主要采用陀螺仪及加速度计进行姿态角、以及加速度的测量定位，但从初始对准开始，其误差会随时间而增大，即出现累计误差大的缺点，另外一些UWB/IMU的常用融合方法面对一方传感器失效状态时无法达到预期效果。

技术实现思路

[0005]本专利技术目的在于提供一种基于超宽带和惯性单元融合的井下移动装备定位方法，不仅避免单一UWB定位造成定位信息跳变，有效收束UWB的定位误差，并避免单一IMU定位造成累积误差，而且可以灵活组合不同量测频率的传感器，实现即插即用的功能。
[0006]为实现上述目的，本一种基于超宽带和惯性单元融合的井下移动装备定位方法，一种基于超宽带和惯性单元融合的井下移动装备定位方法，其特征在于，具体包括以下步骤：
[0007]步骤一，对六路惯性单元数据进行融合并解算：
[0008]通过姿态变换矩阵使得六路惯性单元的三轴角速度和三轴比例加速度均转化为钻机坐标系下的参数，并进行数据融合得到融合比例加速度、融合角速度，解算获得钻机的位姿信息；
[0009]步骤二，将步骤一中惯性单元获得的融合加速度与融合角速度、以及超宽带的三维位置融入因子图，建立IMU/UWB的因子图模型，并获得最小误差函数；
[0010]步骤三，将步骤二中的误差函数作为目标函数通过哈里斯鹰优化算法寻找最优值、进而获得钻机三维坐标。
[0011]进一步的，步骤一中六路惯性单元按正四棱锥空间进行布置，分别位于正四棱锥的五个连接点、以及中心位置；
[0012]正四棱锥中心位置处的惯性单元z轴指向正四棱锥上顶点处，y轴指向正前方，上顶点处惯性单元z轴指向底面、y轴指向正前方，底部四个惯性单元成差分结构分布于正四棱锥底面四个顶点，并且相邻两个惯性单元的z轴指向相反、y轴均指向底面中心点；六路惯性单元的x轴指向均服从右手坐标系原理。
[0013]进一步的，角速度姿态变换矩阵表示为：
[0014][0015]其中，ω
ibx
、ω
iby
、ω
ibz
分别表示钻机坐标系下i位置相应的三轴角速，γ
i
、θ
i
、分别表示惯性单元i相对钻机坐标系三轴旋转的角度，ω
ix
、ω
iy
、ω
iz
分别表示惯性单元所测得三轴角速度；
[0016]比例加速度姿态变换矩阵表示为：
[0017][0018]其中：f
ibx
、f
iby
、f
ibz
分别表示钻机坐标系下i位置相应的三轴比例加速度，γ
i
、θ
i
、分别表示惯性单元i相对钻机坐标系绕三轴旋转的角度，f
ix
、f
iy
、f
iz
分别表示惯性单元所测得三轴比例加速度；
[0019]融合角速度表示为：
[0020][0021]融合比例加速度表示为：
[0022][0023]进一步的，步骤二中，最小误差函数为：
[0024]argmin
x
F(x
1:n
)
[0025][0026]其中，f
prior
为先验因子节点；
[0027]f
bias
(α
k+1
,α
k
)为IMU惯性器件所相关的偏差因子节点；
[0028]f
IMU
(x
k+1
,x
k
,α
k
)为IMU在k时刻的因子节点；
[0029]f
UWB
(x
k
)为UWB在k时刻的因子节点。
[0030]进一步的，先验因子节点所对应的误差函数为：
[0031][0032]其中，μ＝{X1,V1,α1}表示车辆的初始状态，X1、V1、α1分别表示相应状态下防冲钻孔机器人的三维位置、速度和三轴角度的初始估计，Σ
Prior
表示防冲钻孔机器人初始状态的误差协方差矩阵；
[0033]表示初始时刻可用先验信息，和分别表示对防冲钻孔机器人的三维位置、速度和三轴角度的初始估计。
[0034]进一步的，所述IMU在k时刻的因子节点公式为：
[0035][0036]其中，x
k
为当前时刻为k时导航状态量，x
k+1
为下一时刻的导航状态量；
[0037]α
k
为时刻为k时IMU惯性器件所产生的误差变量；
[0038]d()表示连接上下相邻状态的误差函数；
[0039]为IMU测量信息，其公式为：
[0040][0041]f
b
为当前k时刻融合比例加速度；w
b
为当前k时刻融合角速率。
[0042]进一步的，所述UWB在k时刻的因子节点定义为：
[0043][0044]其中，为超宽带的量测方程；
[0045]h
UWB
(x
k
)为UWB预计真值的量测函数。
[0046]进一步的，所述IMU惯性器件所相关的偏差因子节点公式为：
[0047][0048]其中，α
k+1
为时刻为k+1时IMU惯性器件所产生的误差变量；
[0049]g(α
k
)为通过k时刻误差变量计算k+1时刻误差变量的误差更新方程。
[0050]进一步的，步骤三中通过哈里斯鹰优化算法对目标函数寻找最优数值，并获得钻机三维坐标，具体步骤如下：
[0051]步本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种基于超宽带和惯性单元融合的井下移动装备定位方法，其特征在于，具体包括以下步骤：步骤一，对六路惯性单元数据进行融合并解算：通过姿态变换矩阵使得六路惯性单元的三轴角速度和三轴比例加速度均转化为钻机坐标系下的参数，并进行数据融合得到融合比例加速度、融合角速度，解算获得钻机的位姿信息；步骤二，将步骤一中惯性单元获得的融合加速度与融合角速度、以及超宽带的三维位置融入因子图，建立IMU/UWB的因子图模型，并获得最小误差函数；步骤三，将步骤二中的误差函数作为目标函数通过哈里斯鹰优化算法寻找最优值、进而获得钻机三维坐标。2.根据权利要求1所述的一种基于超宽带和惯性单元融合的井下移动装备定位方法，其特征在于，步骤一中六路惯性单元按正四棱锥空间进行布置，分别位于正四棱锥的五个连接点、以及中心位置；正四棱锥中心位置处的惯性单元z轴指向正四棱锥上顶点处，y轴指向正前方，上顶点处惯性单元z轴指向底面、y轴指向正前方，底部四个惯性单元成差分结构分布于正四棱锥底面四个顶点，并且相邻两个惯性单元的z轴指向相反、y轴均指向底面中心点；六路惯性单元的x轴指向均服从右手坐标系原理。3.根据权利要求2所述的一种基于超宽带和惯性单元融合的井下移动装备定位方法，其特征在于，角速度姿态变换矩阵表示为：其中，ω
ibx
、ω
iby
、ω
ibz
分别表示钻机坐标系下i位置相应的三轴角速，γ
i
、θ
i
、分别表示惯性单元i相对钻机坐标系三轴旋转的角度，ω
ix
、ω
iy
、ω
iz
分别表示惯性单元所测得三轴角速度；比例加速度姿态变换矩阵表示为：其中：f
ibx
、f
iby
、f
ibz
分别表示钻机坐标系下i位置相应的三轴比例加速度，γ
i
、θ
i
、分别表示惯性单元i相对钻机坐标系绕三轴旋转的角度，f
ix
、f
iy
、f
iz
分别表示惯性单元所测得
三轴比例加速度；融合角速度表示为：融合比例加速度表示为：4.根据权利要求3所述的一种基于超宽带和惯性单元融合的井下移动装备定位方法，其特征在于，步骤二中，最小误差函数为：argmin
x
F(x
1:n
)其中，f
prior
为先验因子节点；f
bias
(α
k+1
,α
k
)为IMU惯性器件所相关的偏差因子节点；f
IMU
(x
k+1
,x
k
,α
...

【专利技术属性】
技术研发人员：司垒，张鹏辉，王忠宾，梁斌，魏东，顾进恒，刘新华，闫海峰，谭超，
申请(专利权)人：中国矿业大学，
类型：发明
国别省市：