一种基于UWB、IMU的井下胶轮车融合定位方法

一种基于uwb、imu的井下胶轮车融合定位方法
技术领域
1.本发明涉及融合定位技术领域，特别涉及一种基于uwb、imu的井下胶轮车融合定位方法。


背景技术：

2.煤矿运输是井下煤矿生产中不可或缺的一环。随着时代发展与技术进步，无轨胶轮车因其自动驾驶、自由灵活等优点成为了一种新兴井下运输方式，在我国得到了迅速的推广及应用。同时，保证无轨胶轮车的安全、稳定、高效、智能运行与调度等成为业内亟待解决的问题，井下环境无轨胶轮车的实时精确定位则是其实现自动驾驶的基础。
3.近年来，uwb技术因其具有系统实现简单、多径分辨能力强、定位精确度高等优点，在各种室内环境定位中得到广泛应用。imu定位系统集成了加速度计、陀螺仪，磁力计等传感器，具有完全自主导航、数据更新速率高、短期定位精度高，不受外界环境干扰的优点。如何将两种定位技术进行有效融合，提高隧道胶轮车定位的准确性和可靠性，是一个亟待解决的重要课题。井下隧道作为一种特殊的室内环境，环境状况与普通建筑物室内环境有较大不同。普通室内定位一般采用二维或三维定位，被定位的物体多为平面或空间中的移动体；但隧道环境往往为几何位置信息已知的狭长通道，采用一维定位准确确定胶轮车沿隧道延伸方向的相对位置，即可满足定位精度需求。这样系统所需设备数量更少，维护更简单可靠，成本更低，但目前国内还缺乏完整实用的隧道环境车辆一维定位方案。
4.公开号cn114323003a的发明专利，公开了一种基于的矿井无人机车高精度定位系统及方法。该发明采用若干uwb标签节点tag、uwb锚节点anchor，井下通信ap设备以及信标器四个部分完成了矿井下机车位置定位，但定位场景适应度差，且没有详细的uwb基站部署方案以及机车运动过程的基站切换方案。
5.公开号cn113286360a的发明专利，公开了一种基于umb、imu及激光雷达的井工矿融合定位方法。该发明通过融合uwb、imu以及激光雷达三个维度的定位信息构建ekf融合定位模型来实现井下无人驾驶车辆的定位，但是没有考虑到井下定位过程中可能存在的nlos情况，并且没有考虑到激光雷达受井下灰尘影响较大这一问题，导致其输出的定位结果可靠性低，车辆定位精准度低。
6.公开号cn114166221b的发明专利，公开了动态复杂矿井环境中辅助运输机器人定位方法及系统。该发明通过获得辅助运输机器人的加速度信息和uwb距离量测信息，利用扩展卡尔曼滤波得到机器人位置估计，并对uwb量测进行异常值检测，若uwb量测异常，通过修正新息协方差矩阵改进扩展卡尔曼滤波从而提高定位精度。但该发明没有考虑到井下环境的特殊性，测试长度较短，且没有最后的定位结果呈现方案。


技术实现要素：

7.本发明提供了一种基于uwb、imu的井下胶轮车融合定位方法，消除了imu累积误差的不利影响，实现了uwb参考基站的动态切换，提升了定位实时性，具有抗uwb随机测量误差
及nlos误差的能力，可以有效适应隧道内行驶状态下胶轮车的精确定位。
8.为了达到上述目的，本技术提供如下技术方案：
9.一种基于uwb、imu的井下胶轮车融合定位方法，包括以下步骤：
10.s100，在胶轮车上设置车载定位处理装置，所述车载定位处理装置包括uwb标签、imu器件、车载计算机和无线通信模块；所述uwb标签的天线设置在胶轮车顶部；
11.s200，部署uwb基站，所述uwb基站包括uwb起点基站和uwb中间基站；在隧道口部署uwb起点基站，以uwb起点基站为基准，在隧道中部署若干个uwb中间基站；获取各uwb中间基站相对于uwb起点基站的一维位置数据，并存储至车载数据库；
12.s300，uwb标签发出测距信号；各uwb基站接收到测距信号后，采用双边测距法计算与uwb标签的距离，并反馈距离计算结果至uwb标签；车载计算机读取各uwb基站反馈的距离计算结果，并根据uwb标签与各uwb基站的距离，对各uwb基站进行排序，将排序结果存储至车载数据库；
13.s400，车载计算机读取imu器件输出的惯性数据，并将所述惯性数据存储在双端队列中；
14.s500，根据排序结果，筛选两个uwb基站为参考基站，进行uwb一维定位解算，并将生成的uwb位置解算结果经卡尔曼滤波，输出uwb标签的uwb定位解算结果；
15.s600，以uwb定位解算结果为imu初始位置，根据双端队列中的惯性数据，采用机械编排算法进行imu一维定位解算，并生成imu机械编排解算结果；
16.s700，采用卡尔曼滤波器通过松组合的方式，对uwb定位解算结果和imu机械编排解算结果进行融合，并生成融合定位结果；
17.s800，显示融合定位结果，并通过无线通信模块传输至云服务器。
18.进一步，所述uwb基站均部署在隧道的顶部中线上；
19.s200包括：
20.s201，在隧道口部署uwb起点基站；
21.s202，测量隧道内uwb基站与uwb标签的有效测距距离，并根据有效测距距离生成基站间距；所述基站间距小于有效测距距离；
22.s203，根据基站间距，在隧道中部署若干个uwb中间基站；
23.s204，建立二维参考坐标系；以隧道口为参考原点，获取隧道延伸方向，以隧道延伸方向为x轴正方向，隧道口为x轴起点；获取uwb标签的水平高度，以竖直向上方向为y轴正方向，uwb标签的水平高度为y轴起点；
24.s205，根据二维参考坐标系，获取各uwb中间基站相对于uwb起点基站的一维位置数据，并存储至车载数据库。
25.进一步，s300中，uwb标签每隔一预设时间发出测距信号，车载计算机生成一相应的排序结果；
26.根据排序结果，筛选两个uwb基站为参考基站，包括：
27.s1，分析排序结果中uwb基站的数量是否小于三个，若是，则执行s2，若否，则执行s3；
28.s2，若排序结果中uwb基站的数量为两个，则将排序结果中的两个uwb基站分别作为第一参考基站和第二参考基站；
29.s3，将排序结果中，与uwb标签的距离最近的一个uwb基站作为第一参考基站，并将剩余uwb基站中，与uwb标签距离最近的两个uwb基站作为备选基站；
30.s4，判断两备选基站与uwb标签的距离是否相等，若是，则执行s5，若否，则将与uwb标签距离更近的uwb基站作为第二参考基站；
31.s5，分析第一参考基站与uwb标签的距离是否小于距离阈值，若是，则执行s6，若否，则将与uwb标签距离更近的uwb基站作为第二参考基站；
32.s6，获取车载计算机生成的上一个排序结果；分析上一个排序结果与当前的排序结果中，与uwb标签距离最近的三个uwb基站之间的排序是否相同，若是，则将与uwb标签距离更近的uwb基站作为第二参考基站，若否，则将与uwb标签距离更远的uwb基站作为第二参考基站。
33.进一步，s400包括：
34.s401，车载计算机初始化uart，并生成指定端口号和数据传输波特率；
35.s402，读取uart数据，并建立长度为200的双端队列；
36.s403，读取imu器件输出的惯性数据，并将所述惯性数据存储在双端队列中。
37.进一步，s500包括：
38.s501，获取隧道的坡度，分析隧道的坡度是否为0，并生成坡度分析结果；
39.s502，以两个参考基站为圆心，两参考基站与uwb标签的距离为半径，分别绘制两个圆；分析两个圆是否相交，并生成位置关系分析结果；
40.s503，根据坡度分析结果和位置关系分析结果，采用中点求值法或三边求值法，进行uwb一维定位解算，并生成uwb位置解算结果；
41.s504，将生成的uwb位置解算结果经卡尔曼滤波，输出uwb标签的uwb定位解算结果。
42.进一步，s600包括：
43.s601，获取imu器件的俯仰角、横滚角和偏航角，并生成胶轮车由b系到n系的姿态矩阵；
44.b系到n系经过三次旋转，每次旋转对应的变换矩阵为：
[0045][0046]
n系到b系的姿态矩阵为：
[0047][0048]
由于n系到b系的旋转的过程中坐标系始终保持直角坐标系，则根据单位正交矩阵的性质有：
[0049][0050]
b系到n的姿态矩阵为：
[0051][0052]
式中，ψ为俯仰角；θ为横滚角；γ为偏航角；n为n系；b为b系；
[0053]
s602，获取imu器件的加速度，经与姿态矩阵相乘后二次积分，计算出胶轮车在t时刻相对于t-1时刻的相对位置；
[0054]
imu器件的加速度经过坐标转换后为：
[0055][0056]
经过时间δt后，胶轮车的速度为：
[0057][0058]
经过时间δt后，胶轮车的位置为：
[0059][0060]
式中，为t时刻b系加速度，为b系到n系的姿态转换矩阵，为t时刻n系加速度，为t-1时刻n系下胶轮车速度，为t时刻n系下胶轮车速度，为t-1时刻n系下胶轮车位置，为t时刻n系下胶轮车位置。
[0061]
进一步，s700还包括：将融合定位结果存储至车载数据库；
[0062]
采用卡尔曼滤波器通过松组合的方式，对uwb定位解算结果和imu机械编排解算结果进行融合，并生成融合定位结果，包括：
[0063]
建立uwb、imu松组合定位系统的离散状态空间模型：
[0064]
状态方程：xk＝fx
k-1
+w
k-1
[0065]
测量方程：zk＝hxk+vk[0066]
状态一步预测：x
k|k-1
＝fx
k-1|k-1
[0067]
一步预测协方差：p
k|k-1
＝fp
k-1|k-1ft
+q
[0068]
滤波增益：kk＝p
k|k-1ht
(hp
k|k-1ht
+r)-1
[0069]
状态估计：xk＝x
k|k-1
+kk(z
k-hx
k|k-1
)
[0070]
估计状态协方差：pk＝p
k|k-1
+kkhp
k|k-1
[0071]
其中，
[0072]
式中，k为当前采样时刻；t为采样周期；w为系统的过程噪声，设为协方差为q的白噪声序列；v为系统的测量噪声，设为协方差为r的白噪声序列，w与v互不相关，xk为k时刻系统状态变量，f为状态转移矩阵，zk为k时刻系统测量变量，h为观测矩阵，pk为k时刻误差协方差矩阵，kk为k时刻滤波增益，s
x
为x方向位移，v
x
为x方向速度,为x方向uwb推算得到的位移；
[0073]
s600中，以uwb定位解算结果或融合定位结果为imu初始位置，根据双端队列中的惯性数据，采用机械编排算法进行imu一维定位解算。
[0074]
进一步，所述uwb标签的天线设置在隧道地面中线的上方。
[0075]
进一步，所述胶轮车上设有车载电子地图；
[0076]
s800中，车载计算机将融合定位结果发送至车载电子地图进行显示，并将融合定位结果通过无线通信模块传输至云服务器。
[0077]
本发明的原理及优点在于：
[0078]
1、结合隧道狭长的环境特点，在隧道中部署uwb基站，并在胶轮车上设置uwb标签，从而可以通过计算uwb标签与各uwb基站的距离，实现胶轮车的uwb一维定位解算。在初始状态下，将uwb定位解算结果作为imu初始位置，在已有生成的融合定位结果时，将融合定位解算结果作为imu初始位置，再根据双端队列中的惯性数据，采用机械编排算法实现了imu一维定位解算，并有效消除了imu累积误差的不利影响。最后采用卡尔曼滤波器通过松组合的方式，对uwb定位解算结果和imu机械编排解算结果进行融合，并生成融合定位结果，实现了隧道内行驶状态下胶轮车的精确定位。
[0079]
2、将uwb标签的天线设置在胶轮车顶部，减少了uwb标签与uwb基站之间的遮挡，从而避免uwb基站与uwb标签之间信号出现nlos非视距传输，由此防止uwb测距出现nlos误差。又通过对隧道坡度的测量，在uwb位置解算过程中引入坡度影响因素，分析出实际距离在一维定位解算中的映射关系，区分直道、坡道情况下的计算方法，提升了定位结果的准确性。
[0080]
3、根据有效测距距离生成基站间距，且基站间距小于有效测距距离，从而可以防止uwb基站之间的间距过大，导致uwb标签在隧道中无法同时获取到两个uwb基站的信号，以至于无法进行位置计算；同时防止uwb基站之间间距过小，所需设置的基站数量过多，从而可以降低基站设置数量，降低运营及维护的成本，并避免基站冗余造成的通信拥堵。
[0081]
4、将uwb位置解算结果经过卡尔曼滤波器滤波后，再与imu机械编排解算结果进行松组合融合，提升了uwb定位精度及融合定位精度；通过引入uwb绝对位置定位信息来补偿imu定位的累积误差，引入imu的自主测量信息抵御nlos可能造成的较大uwb定位误差，实现了uwb与imu两者之间的取长补短，提高了定位的准确性和可靠性。
[0082]
综上，采用本方案，消除了imu累积误差的不利影响，实现了uwb参考基站的动态切换，提升了定位实时性，具有抗uwb随机测量误差及nlos误差的能力，可以有效适应隧道内行驶状态下胶轮车的精确定位。
附图说明
[0083]
图1为本发明实施例一种基于uwb、imu的井下胶轮车融合定位方法的流程示意图。
[0084]
图2为本发明实施例一种基于uwb、imu的井下胶轮车融合定位方法中车载定位处理装置的处理逻辑示意图。
[0085]
图3为本发明实施例一种基于uwb、imu的井下胶轮车融合定位方法中uwb基站部署方法示意图。
[0086]
图4为本发明实施例一种基于uwb、imu的井下胶轮车融合定位方法中uwb基站与uwb标签通信过程示意图。
[0087]
图5为本发明实施例一种基于uwb、imu的井下胶轮车融合定位方法中基站选择与确认方法示意图。
[0088]
图6为本发明实施例一种基于uwb、imu的井下胶轮车融合定位方法中直道情况下
uwb标签与uwb基站的位置示意图。
[0089]
图7为本发明实施例一种基于uwb、imu的井下胶轮车融合定位方法中直道情况下uwb标签与uwb基站的几何关系示意图。
[0090]
图8为本发明实施例一种基于uwb、imu的井下胶轮车融合定位方法中坡道情况下uwb标签与uwb基站的位置示意图。
[0091]
图9为本发明实施例一种基于uwb、imu的井下胶轮车融合定位方法中坡道情况下uwb标签与uwb基站的几何关系示意图。
[0092]
图10为本发明实施例一种基于uwb、imu的井下胶轮车融合定位方法中隧道与uwb基站的立体结构示意图。
[0093]
图11为本发明实施例一种基于uwb、imu的井下胶轮车融合定位方法中隧道与uwb基站的平面结构示意图。
具体实施方式
[0094]
下面通过具体实施方式进一步详细说明：
[0095]
实施例1：
[0096]
一种基于uwb、imu的井下胶轮车融合定位方法，如图1所示，包括以下步骤：
[0097]
s100，在胶轮车上设置车载定位处理装置，所述车载定位处理装置包括uwb标签、imu器件、车载计算机和无线通信模块。为了避免在隧道内受到严重的非视距影响，所述uwb标签的天线设置在胶轮车顶部，以减少uwb标签与uwb基站之间的遮挡，且所述uwb标签位于隧道地面中线的上方。将imu器件固定安装在胶轮车内，防止胶轮车与imu器件发生相对运动。本实施例中，所述uwb标签采用与基站相同型号的uwb芯片，imu器件采用jy901九轴惯性传感器，无线通信模块采用wifi通信模块，在本技术的其他实施例中，还可以采用5g通信模块。由uwb标签、imu器件、车载计算机和无线通信模块组成的车载定位处理装置如图2所示，由车载电源供电，可通过wifi/5g通信模块将数据传输至云服务器，再通过云服务器传输至车辆位置显示终端，便于远程监控。
[0098]
s200，部署uwb基站，所述uwb基站安装在隧道顶部墙面中线处，使其与uwb标签保持在同一立平面内，安装示意图见图3所示，图三中，上方的图示为直道中基站的安装示意图，下方的图示为坡道中基站的安装示意图。uwb基站与uwb标签可接受的水平位置差为uwb标签可略偏离隧道中线(位移差小于1m)，uwb基站配置uwb芯片和全向天线。
[0099]
所述uwb基站包括uwb起点基站和uwb中间基站；在隧道口部署uwb起点基站，以uwb起点基站为基准，在隧道中部署若干个uwb中间基站；获取各uwb中间基站相对于uwb起点基站的一维位置数据，并存储至车载数据库；所述uwb基站均部署在隧道的顶部中线上。
[0100]
本实施例中，如图10所示，本实施例中隧道由直道和坡道两部分组成，隧道高5m，宽5m，直道部分长度为1000m，坡道部分长500m，倾角为10
°
。假定测量得到的距离d为100m，胶轮车顶天线高度2.5m。如图10、图11所示，从隧道口开始，将uwb基站布置在隧道顶部中线上，保持统一水平间隔70m(0.7d)。
[0101]
s200包括：
[0102]
s201，在隧道口部署uwb起点基站；本实施例中，在隧道入口部署uwb起点基站，在所有中间基站部署完毕后，再在隧道出口出书uwb终点基站，基站和基站保持在同一隧道立
平面。
[0103]
s202，测量隧道内uwb基站与uwb标签的有效测距距离，并根据有效测距距离生成基站间距；所述基站间距小于有效测距距离。具体的，在隧道中现场试验测试确定uwb平均测距误差小于80cm的最大通信距离d，本实施例中，有效测距距离为0.8d，基站间距为0.7d-0.8d。所述基站间距与隧道的宽度成正比，保证至少同时有两个uwb基站与uwb标签通信测距时不受隧道壁的遮挡。
[0104]
s203，根据基站间距，在隧道中部署若干个uwb中间基站。
[0105]
s204，建立二维参考坐标系；以隧道口为参考原点，获取隧道延伸方向，以隧道延伸方向为x轴正方向，隧道口为x轴起点；获取uwb标签的水平高度，以竖直向上方向为y轴正方向，uwb标签的水平高度为y轴起点。
[0106]
s205，根据二维参考坐标系，获取各uwb中间基站相对于uwb起点基站的一维位置数据，并存储至车载数据库。
[0107]
s300，uwb标签发出测距信号；各uwb基站接收到测距信号后，采用双边测距法计算与uwb标签的距离，并反馈距离计算结果至uwb标签；车载计算机读取各uwb基站反馈的距离计算结果，并根据uwb标签与各uwb基站的距离，对各uwb基站进行排序，将排序结果存储至车载数据库，具体的，基于uwb标签与uwb标签之间距离由小到大的顺序，对uwb基站进行优选排序；本实施例中，uwb标签每隔一预设时间发出测距信号，车载计算机生成一相应的排序结果。
[0108]
本实施例中，测距方法采用基于toa的ads-twr测距算法，该过程包括uwb基站和uwb标签之间的三次通信，通信过程如图4所示。
[0109]
(1)第一次通信：uwb标签初始化后，向周围的基站广播发送poll数据帧，poll数据帧中含有标签id等信息，此时uwb标签会记录发送poll数据帧时的时间戳tsp。uwb基站初始化后为接收模式，用来接收uwb标签发来的数据帧，接收到poll数据帧后会记录信号到达时的时间戳trp；
[0110]
(2)第二次通信：uwb基站在收到poll数据帧后经过一个时间间隔tsrpa向uwb标签发送response数据帧并标记好发送时间戳tsr，其中tsrpa包括处理poll信号和生成response数据帧的时间，uwb标签接收到response数据帧后记录接收时间戳trr；
[0111]
(3)第三次通信：uwb标签经过一个时间间隔tsrpt后向基站发送final数据帧并记录发送时的时间戳tsf，final数据帧中包含标签id、tsp、trr、tsp信息，uwb基站接收到final信号后会记录接收时的时间戳trf；
[0112]
tsp：发送poll数据帧时的时间戳，完成标签初始化后，标签向周围的基站广播发送poll数据帧，poll数据帧中含有标签id等信息；
[0113]
trp：基站接收到poll数据帧后记录信号到达时的时间戳trp；
[0114]
tsrpa：一个时间间隔，包括处理poll信号和生成response数据帧的时间；
[0115]
tsr：发送时间戳tsr，在基站在收到poll数据帧后经过时间间隔tsrpa向标签发送response数据帧；
[0116]
trr：接收时间戳，标签接收到response数据帧后记录；
[0117]
tsrpt：一个时间间隔；
[0118]
tsf：发送时间戳tsf，基站经过一个时间间隔tsrpt发送final数据帧后记录，
final数据帧中包含标签id、tsp、trr、tsp信息；
[0119]
trf：接收时间戳，基站接收到final信号后记录；
[0120]
uwb基站根据时间戳计算出信号在uwb基站和uwb标签之间的飞行时间，计算公式如下：
[0121][0122][0123]
信号的飞行时间乘以光速基站可以得到uwb标签与uwb基站之间的距离值，uwb该距离放在response数据帧里发送到uwb假定井下胶轮车行驶的最高时速为40km/h，一般时速为20km/h，建议uwb标签与uwb基站的测距速率为10hz，即每秒进行10次测距定位。
[0124]
车载计算机从uwb标签接收的各uwb基站response数据包中，读取uwb基站在前一次测距解算的uwb标签与该uwb基站之间的距离数据，将基站id及其位置坐标、距离数据存放在计算机的缓存中。
[0125]
s400，车载计算机读取imu器件输出的惯性数据，并将所述惯性数据存储在双端队列中，以便后续胶轮车的速度与位置解算；具体的，车载计算机与imu器件使用uart进行数据通信，通过数据头区分加速度、角速度、欧拉角等惯性信息，使用长度为200的双端队列对imu数据进行存储。
[0126]
s400包括：
[0127]
s401，车载计算机初始化uart，并生成指定端口号和数据传输波特率。
[0128]
s402，读取uart数据，并建立长度为200的双端队列。
[0129]
s403，读取imu器件输出的惯性数据，并将所述惯性数据存储在双端队列中。
[0130]
s500，根据排序结果，筛选两个uwb基站为参考基站，进行uwb一维定位解算，并将生成的uwb位置解算结果经卡尔曼滤波，输出uwb标签的uwb定位解算结果。
[0131]
根据排序结果，动态筛选两个uwb基站为参考基站，包括：
[0132]
s1，分析排序结果中uwb基站的数量是否小于三个，若是，则执行s2，若否，则执行s3。
[0133]
s2，若排序结果中uwb基站的数量为两个，则将排序结果中的两个uwb基站分别作为第一参考基站和第二参考基站。
[0134]
s3，将排序结果中，与uwb标签的距离最近的一个uwb基站作为第一参考基站，并将剩余uwb基站中，与uwb标签距离最近的两个uwb基站作为备选基站；如图5所示，基于uwb标签与uwb基站之间距离由小到大的顺序，得到距离序列r1，r2，r3及对应的基站序列a，b，c。
[0135]
s4，判断两备选基站与uwb标签的距离是否相等，若是，则执行s5，若否，则将与uwb标签距离更近的uwb基站作为第二参考基站，也即若r3-r2》0，则将b选为第二参考基站。
[0136]
s5，分析第一参考基站与uwb标签的距离是否小于距离阈值(仅在uwb标签距离uwb基站较近时，才考虑基站切换的问题)，若是，则执行s6，若否，则将与uwb标签距离更近的uwb基站作为第二参考基站。
[0137]
s6，获取车载计算机生成的上一个排序结果；分析上一个排序结果与当前的排序结果中，与uwb标签距离最近的三个uwb基站之间的排序是否相同，若是，则将与uwb标签距离更近的uwb基站作为第二参考基站，若否，则将与uwb标签距离更远的uwb基站作为第二参
考基站，本实施例中：
[0138]
(1)若r1小于等于5m，即uwb标签靠近基站a时，为消除随机测量误差可能造成的误切换，前一时刻的基站序列为a，b，c，当前时刻变为a，c，b，继续选a和b，只有当下一时刻基站序列仍然为a,c,b时，才选a和c实现基站切换。
[0139]
(2)若r1大于5m，即uwb标签离基站a较远时，前一时刻的基站序列为a，b，c，当前时刻变为a，c，b，说明当前时刻基站b的测距出现较大的nlos误差，此时不能执行基站切换及一维定位解算，应继续选a和b作为定位基站，但uwb标签的定位需由前一时刻的uwb标签位置及胶轮车当前行驶速度推算确定。
[0140]
s500包括：
[0141]
s501，获取隧道的坡度，分析隧道的坡度是否为0，并生成坡度分析结果。
[0142]
s502，以两个参考基站为圆心，两参考基站与uwb标签的距离为半径，分别绘制两个圆；分析两个圆是否相交，并生成位置关系分析结果。
[0143]
s503，根据坡度分析结果和位置关系分析结果，采用中点求值法或三边求值法，进行uwb一维定位解算，并生成uwb位置解算结果。
[0144]
如图6、图7所示，若所述坡度分析结果为隧道为水平直道，则以两个参考基站为圆心，两参考基站与uwb标签的距离为半径，分别绘制两个圆。根据测距误差的不同(两测距值之和大于、等于、小于两基站之间的距离)，首先分析两个圆是否相交；
[0145]
若是(两圆相交的情况)，则uwb标签相对参考基站的一维位置计算公式如下：(利用三角形三边与夹角关系求解出标签c相对基站a一维位置)
[0146][0147][0148]
xd＝ad
[0149]
若否(两圆相离或相切的情况)，则先求出两圆与ab交点的坐标值，再利用两交点的x轴坐标的中点作为移动标签的x轴位置，即点d的横坐标：
[0150][0151]
本实施例中：
[0152]
d：点c在ab上的竖直投影；
[0153]
ab：基站a与基站b连线的长度；
[0154]
α：标签c与基站a之间的角度；
[0155]
r1：标签c与基站a之间的距离值；
[0156]
r2：标签c与基站b之间的距离值；
[0157]
xd：标签c相对基站a的水平距离，套用公式计算结果；
[0158]
如图8、图9所示，若所述坡度分析结果为隧道为坡道，则以两个参考基站为圆心，两参考基站与uwb标签的距离为半径，分别绘制两个圆，首先判断两圆的关系为相切和相离中的哪一种。
[0159]
两圆相离(相切)的情况，可以考虑先求出两圆与ab交点的坐标值，再利用两交点
的x轴坐标的中点作为移动标签的x轴位置，即点d的横坐标：
[0160]
xd＝d
·
cosβ
[0161]
两圆相交的情况，根据三角形三边关系求解出标签c相对基站a一维位置：
[0162][0163][0164]
xd＝d
·
cosβ
[0165]
本实施例中：
[0166]
d：点c在ab上的竖直投影；
[0167]
ab：基站a与基站b连线的长度；
[0168]
dc：点c与点d之间连线的长度
[0169]
α：标签c与基站a之间的角度；
[0170]
β：坡道的倾角角度；
[0171]
r1：标签c与基站a之间的距离值；
[0172]
r2：标签c与基站b之间的距离值；
[0173]
xd：标签c相对基站a的水平距离，套用公式计算结果；
[0174]
s504，将生成的uwb位置解算结果经卡尔曼滤波，降低随机噪声的不利影响，输出uwb标签的uwb定位解算结果，具体的：
[0175]
状态一步预测：x
k|k-1
＝fx
k-1|k-1
[0176]
一步预测协方差：p
k|k-1
＝fp
k-1|k-1ft
+q
[0177]
滤波增益：kk＝p
k|k-1ht
(hp
k|k-1ht
+r)-1
[0178]
状态估计：xk＝x
k|k-1
+kk(z
k-hx
k|k-1
)
[0179]
估计状态协方差：pk＝p
k|k-1
+kkhp
k|k-1
[0180]
其中，观测量状态量观测矩阵为h＝[1 0]，过程噪声协方差矩阵测量噪声协方差矩阵r＝[0.152]。
[0181]
式中，k为当前采样时刻；t为采样周期，xk为k时刻系统状态变量，f为状态转移矩阵，zk为k时刻系统测量变量，h为观测矩阵，pk为k时刻误差协方差矩阵，kk为k时刻滤波增益，s
x
为x方向位移，v
x
为x方向速度,为x方向uwb推算得到的位移。
[0182]
s600，以uwb定位解算结果或融合定位结果为imu初始位置，根据双端队列中的惯性数据，采用机械编排算法进行imu一维定位解算，并生成imu机械编排解算结果；
[0183]
s600包括：
[0184]
s601，获取imu器件的俯仰角、横滚角和偏航角，并生成胶轮车由b系到n系的姿态矩阵；
[0185]
b系到n系经过三次旋转，每次旋转对应的变换矩阵为：
[0186][0187]
n系到b系的姿态矩阵为：
[0188][0189]
由于n系到b系的旋转的过程中坐标系始终保持直角坐标系，则根据单位正交矩阵的性质有：
[0190][0191]
b系到n的姿态矩阵为：
[0192][0193]
式中，ψ为俯仰角；θ为横滚角；γ为偏航角；n为n系；b为b系；
[0194]
s602，获取imu器件的加速度，经与姿态矩阵相乘后二次积分，计算出胶轮车在t时刻相对于t-1时刻的相对位置；
[0195]
imu器件的加速度经过坐标转换后为：
[0196][0197]
经过时间δt后，胶轮车的速度为：
[0198][0199]
经过时间δt后，胶轮车的位置为：
[0200][0201]
式中，为t时刻b系加速度，为b系到n系的姿态转换矩阵，为t时刻n系加速度，为t-1时刻n系下胶轮车速度，为t时刻n系下胶轮车速度，为t-1时刻n系下胶轮车位置，为t时刻n系下胶轮车位置。
[0202]
胶轮车进入隧道口后首先开启uwb定位，连续记录5s uwb定位位置信息，将第5s的位置信息作为imu机械编排解算的初始位置。
[0203]
s700，采用卡尔曼滤波器通过松组合的方式，对uwb定位解算结果和imu机械编排解算结果进行融合，并生成融合定位结果；将融合定位结果存储至车载数据库；
[0204]
采用卡尔曼滤波器通过松组合的方式，对uwb定位解算结果和imu机械编排解算结果进行融合，并生成融合定位结果，包括：
[0205]
以imu的前向位置和前向速度作为状态变量、uwb定位解算结果为测量变量，建立uwb、imu松组合定位系统的离散状态空间模型：
[0206]
状态方程：xk＝fx
k-1
+w
k-1
[0207]
测量方程：zk＝hxk+vk[0208]
状态一步预测：x
k|k-1
＝fx
k-1|k-1
[0209]
一步预测协方差：p
k|k-1
＝fp
k-1|k-1ft
+q
[0210]
滤波增益：kk＝p
k|k-1ht
(hp
k|k-1ht
+r)-1
[0211]
状态估计：xk＝x
k|k-1
+kk(z
k-hx
k|k-1
)
[0212]
估计状态协方差：pk＝p
k|k-1
+kkhp
k|k-1
[0213]
其中，其中，h＝[1 0]，过程噪声协方测量噪声协方差矩阵r＝[0.12]；
[0214]
式中，k为当前采样时刻；t为采样周期；w为系统的过程噪声，设为协方差为q的白噪声序列；v为系统的测量噪声，设为协方差为r的白噪声序列，w与v互不相关，xk为k时刻系统状态变量，f为状态转移矩阵，zk为k时刻系统测量变量，h为观测矩阵，pk为k时刻误差协方差矩阵，kk为k时刻滤波增益，s
x
为x方向位移，v
x
为x方向速度,为x方向uwb推算得到的位移。
[0215]
完成融合定位计算后，将该融合定位结果作为下一时刻imu推算定位的初始位置，并将该位置信息存储在车载计算机内存中，以便后续向监控中心发送定位信息。融合定位系统定位处理流程如图1所示。
[0216]
s800，显示融合定位结果，并通过无线通信模块传输至云服务器。所述胶轮车上设有车载电子地图。车载计算机将融合定位结果发送至车载电子地图进行显示，并将融合定位结果通过无线通信模块传输至云服务器，由此地面监控中心可以通过云服务器获取到胶轮车的位置信息并进行显示。
[0217]
采用本方案，消除了imu累积误差的不利影响，实现了uwb参考基站的动态切换，提升了定位实时性，具有抗uwb随机测量误差及nlos误差的能力，可以有效适应隧道内行驶状态下胶轮车的精确定位。
[0218]
以上的仅是本发明的实施例，方案中公知的具体结构及特性等常识在此未作过多描述，所属领域普通技术人员知晓申请日或者优先权日之前发明所属技术领域所有的普通技术知识，能够获知该领域中所有的现有技术，并且具有应用该日期之前常规实验手段的能力，所属领域普通技术人员可以在本技术给出的启示下，结合自身能力完善并实施本方案，一些典型的公知结构或者公知方法不应当成为所属领域普通技术人员实施本技术的障碍。应当指出，对于本领域的技术人员来说，在不脱离本发明结构的前提下，还可以作出若干变形和改进，这些也应该视为本发明的保护范围，这些都不会影响本发明实施的效果和专利的实用性。本技术要求的保护范围应当以其权利要求的内容为准，说明书中的具体实施方式等记载可以用于解释权利要求的内容。

技术特征：
1.一种基于uwb、imu的井下胶轮车融合定位方法，其特征在于：包括以下步骤：s100，在胶轮车上设置车载定位处理装置，所述车载定位处理装置包括uwb标签、imu器件、车载计算机和无线通信模块；所述uwb标签的天线设置在胶轮车顶部；s200，部署uwb基站，所述uwb基站包括uwb起点基站和uwb中间基站；在隧道口部署uwb起点基站，以uwb起点基站为基准，在隧道中部署若干个uwb中间基站；获取各uwb中间基站相对于uwb起点基站的一维位置数据，并存储至车载数据库；s300，uwb标签发出测距信号；各uwb基站接收到测距信号后，采用双边测距法计算与uwb标签的距离，并反馈距离计算结果至uwb标签；车载计算机读取各uwb基站反馈的距离计算结果，并根据uwb标签与各uwb基站的距离，对各uwb基站进行排序，将排序结果存储至车载数据库；s400，车载计算机读取imu器件输出的惯性数据，并将所述惯性数据存储在双端队列中；s500，根据排序结果，筛选两个uwb基站为参考基站，进行uwb一维定位解算，并将生成的uwb位置解算结果经卡尔曼滤波，输出uwb标签的uwb定位解算结果；s600，以uwb定位解算结果为imu初始位置，根据双端队列中的惯性数据，采用机械编排算法进行imu一维定位解算，并生成imu机械编排解算结果；s700，采用卡尔曼滤波器通过松组合的方式，对uwb定位解算结果和imu机械编排解算结果进行融合，并生成融合定位结果；s800，显示融合定位结果，并通过无线通信模块传输至云服务器。2.根据权利要求1所述的基于uwb、imu的井下胶轮车融合定位方法，其特征在于：所述uwb基站均部署在隧道的顶部中线上；s200包括：s201，在隧道口部署uwb起点基站；s202，测量隧道内uwb基站与uwb标签的有效测距距离，并根据有效测距距离生成基站间距；所述基站间距小于有效测距距离；s203，根据基站间距，在隧道中部署若干个uwb中间基站；s204，建立二维参考坐标系；以隧道口为参考原点，获取隧道延伸方向，以隧道延伸方向为x轴正方向，隧道口为x轴起点；获取uwb标签的水平高度，以竖直向上方向为y轴正方向，uwb标签的水平高度为y轴起点；s205，根据二维参考坐标系，获取各uwb中间基站相对于uwb起点基站的一维位置数据，并存储至车载数据库。3.根据权利要求2述的基于uwb、imu的井下胶轮车融合定位方法，其特征在于：s300中，uwb标签每隔一预设时间发出测距信号，车载计算机生成一相应的排序结果；根据排序结果，筛选两个uwb基站为参考基站，包括：s1，分析排序结果中uwb基站的数量是否小于三个，若是，则执行s2，若否，则执行s3；s2，若排序结果中uwb基站的数量为两个，则将排序结果中的两个uwb基站分别作为第一参考基站和第二参考基站；s3，将排序结果中，与uwb标签的距离最近的一个uwb基站作为第一参考基站，并将剩余uwb基站中，与uwb标签距离最近的两个uwb基站作为备选基站；
s4，判断两备选基站与uwb标签的距离是否相等，若是，则执行s5，若否，则将与uwb标签距离更近的uwb基站作为第二参考基站；s5，分析第一参考基站与uwb标签的距离是否小于距离阈值，若是，则执行s6，若否，则将与uwb标签距离更近的uwb基站作为第二参考基站；s6，获取车载计算机生成的上一个排序结果；分析上一个排序结果与当前的排序结果中，与uwb标签距离最近的三个uwb基站之间的排序是否相同，若是，则将与uwb标签距离更近的uwb基站作为第二参考基站，若否，则将与uwb标签距离更远的uwb基站作为第二参考基站。4.根据权利要求1所述的基于uwb、imu的井下胶轮车融合定位方法，其特征在于：s400包括：s401，车载计算机初始化uart，并生成指定端口号和数据传输波特率；s402，读取uart数据，并建立长度为200的双端队列；s403，读取imu器件输出的惯性数据，并将所述惯性数据存储在双端队列中。5.根据权利要求3所述的基于uwb、imu的井下胶轮车融合定位方法，其特征在于：s500包括：s501，获取隧道的坡度，分析隧道的坡度是否为0，并生成坡度分析结果；s502，以两个参考基站为圆心，两参考基站与uwb标签的距离为半径，分别绘制两个圆；分析两个圆是否相交，并生成位置关系分析结果；s503，根据坡度分析结果和位置关系分析结果，采用中点求值法或三边求值法，进行uwb一维定位解算，并生成uwb位置解算结果；s504，将生成的uwb位置解算结果经卡尔曼滤波，输出uwb标签的uwb定位解算结果。6.根据权利要求1所述的基于uwb、imu的井下胶轮车融合定位方法，其特征在于：s600包括：s601，获取imu器件的俯仰角、横滚角和偏航角，并生成胶轮车由b系到n系的姿态矩阵；b系到n系经过三次旋转，每次旋转对应的变换矩阵为：n系到b系的姿态矩阵为：由于n系到b系的旋转的过程中坐标系始终保持直角坐标系，则根据单位正交矩阵的性质有：b系到n的姿态矩阵为：
式中，ψ为俯仰角；θ为横滚角；γ为偏航角；n为n系；b为b系；s602，获取imu器件的加速度，经与姿态矩阵相乘后二次积分，计算出胶轮车在t时刻相对于t-1时刻的相对位置；imu器件的加速度经过坐标转换后为：经过时间δt后，胶轮车的速度为：经过时间δt后，胶轮车的位置为：式中，为t时刻b系加速度，为b系到n系的姿态转换矩阵，为t时刻n系加速度，为t-1时刻n系下胶轮车速度，为t时刻n系下胶轮车速度，为t-1时刻n系下胶轮车位置，为t时刻n系下胶轮车位置。7.根据权利要求1所述的基于uwb、imu的井下胶轮车融合定位方法，其特征在于：s700还包括：将融合定位结果存储至车载数据库；采用卡尔曼滤波器通过松组合的方式，对uwb定位解算结果和imu机械编排解算结果进行融合，并生成融合定位结果，包括：建立uwb、imu松组合定位系统的离散状态空间模型：状态方程：x
k
＝fx
k-1
+w
k-1
测量方程：z
k
＝hx
k
+v
k
状态一步预测：x
k|k-1
＝fx
k-1|k-1
一步预测协方差：p
k|k-1
＝fp
k-1|k-1
f
t
+q滤波增益：k
k
＝p
k|k-1
h
t
(hp
k|k-1
h
t
+r)-1
状态估计：x
k
＝x
k|k-1
+k
k
(z
k-hx
k|k-1
)估计状态协方差：p
k
＝p
k|k-1
+k
k
hp
k|k-1
其中，h＝[1 0]；式中，k为当前采样时刻；t为采样周期；w为系统的过程噪声，设为协方差为q的白噪声序列；v为系统的测量噪声，设为协方差为r的白噪声序列，w与v互不相关，x
k
为k时刻系统状态变量，f为状态转移矩阵，z
k
为k时刻系统测量变量，h为观测矩阵，p
k
为k时刻误差协方差矩阵，k
k
为k时刻滤波增益，s
x
为x方向位移，v
x
为x方向速度,为x方向uwb推算得到的位移；s600中，以uwb定位解算结果或融合定位结果为imu初始位置，根据双端队列中的惯性数据，采用机械编排算法进行imu一维定位解算。8.根据权利要求1所述的基于uwb、imu的井下胶轮车融合定位方法，其特征在于：所述uwb标签的天线设置在隧道地面中线的上方。9.根据权利要求1所述的基于uwb、imu的井下胶轮车融合定位方法，其特征在于：所述
胶轮车上设有车载电子地图；s800中，车载计算机将融合定位结果发送至车载电子地图进行显示，并将融合定位结果通过无线通信模块传输至云服务器。

技术总结
本发明涉及融合定位技术领域，尤其是一种基于UWB、IMU的井下胶轮车融合定位方法，包括：S100，设置车载定位处理装置；S200，部署UWB基站；S300，UWB标签发出测距信号；车载计算机读取各UWB基站反馈的距离计算结果，并对各UWB基站进行排序；S400，将惯性数据存储在双端队列中；S500，根据排序结果，筛选两个UWB基站为参考基站，进行UWB一维定位解算，输出UWB标签的UWB定位解算结果；S600，根据双端队列中的惯性数据，采用机械编排算法进行IMU一维定位解算，并生成IMU机械编排解算结果；S700，对UWB定位解算结果和IMU机械编排解算结果进行融合，并生成融合定位结果。采用本方案，提升了定位实时性，实现了隧道内行驶状态下胶轮车的精确定位。位。位。


技术研发人员：郭子文 邓平 华锋 刘香渝 周继华 赵涛 陈柯
受保护的技术使用者：西南交通大学
技术研发日：2023.05.25
技术公布日：2023/8/14