一种井下无人飞行器高精度融合定位系统及方法与流程

本发明涉及无人飞行器定位，尤其涉及一种井下无人飞行器高精度融合定位系统及方法。

背景技术：

1、煤矿井下智能无人飞行器巡检系统的提出和发展是国内外煤矿企业智能化综合煤矿开采中重要组成，它将为传统的人工巡检和轨道机器人巡检提供了更安全、更高效、更灵活、更具成本效益的替代方案。将地面飞行器发展成一种适用于煤矿井下特殊环境(空间受限、高粉尘、高可燃气体等)的高速智能无人飞行器巡检系统的首要前提是其高度的自主定位与导航能力。这是其完成煤矿井下带式输送机、变电所、水泵房及其他重点区域自动化巡检，满足无人值守、实时监控、全天候工作要求的基本能力，也是所有室内自主无人飞行器系统的核心研究问题。在煤矿井下等没有gps或卫星信号的室内环境中，配备了先进传感器如相机、惯导、lidar、超声波、超宽带和红外线等的无人飞行器进行巡检的无人飞行器定位和导航技术已经受到了越来越多的关注和研究。但是将其用在室内或者更为特殊的煤矿井下解决无人飞行器的高精度自主定位与导航飞行和巡检中都还处于起步阶段，也无成熟的技术方案。

2、在现有的技术中，井下无人飞行器的定位主要依赖于单一的传感器或简单的传感器组合，这些方案虽然能够在一定程度上实现定位，但精度和稳定性往往达不到实际应用的要求。

3、以下为现有的主要定位技术：

4、(1)基于uwb的定位系统：

5、技术措施：uwb技术通过发送和接收纳秒级的脉冲信号来进行测距，从而实现定位。在井下环境中，可以布置多个uwb基站，无人飞行器搭载uwb标签，通过与基站之间的信号传输时间差来计算无人飞行器的位置。

6、产品构成：主要由uwb基站、无人飞行器搭载的uwb标签以及数据处理单元组成。

7、相互关系：uwb基站负责发送和接收脉冲信号；uwb标签安装在无人飞行器上，用于与基站进行通信；数据处理单元对接收到的信号进行处理，计算出无人飞行器的位置。

8、(2)基于ins和激光传感器的定位系统：

9、技术措施：ins通过陀螺仪和加速度计等惯性传感器来测量无人飞行器的角速度和加速度，从而推算出无人飞行器的位置和姿态。激光传感器则通过扫描周围环境，获取环境的三维信息，辅助ins进行定位。

10、产品构成：主要由ins模块、激光传感器以及数据处理单元组成。

11、相互关系：ins模块提供无人飞行器的姿态和位置初步估计；激光传感器扫描周围环境，提供精确的环境信息；数据处理单元融合ins和激光传感器的数据，提高定位精度。

12、然而，这些现有技术都存在一定的局限性。例如，uwb定位在信号遮挡严重或基站布局不合理的情况下，定位精度会大幅下降；而ins和激光传感器的组合在长时间运行后，由于误差累积，定位精度也会逐渐降低。

13、在井下这种没有卫星信号的环境中，现有的定位技术虽然能够在一定程度上实现无人飞行器的定位，但仍存在一些显著的缺陷：

14、(1)uwb定位技术的缺陷：

15、信号穿透能力有限：uwb信号在遇到障碍物时，信号强度会大幅衰减，影响定位精度。在井下环境中，由于巷道结构复杂，信号遮挡严重，这可能导致定位不准确甚至失败。

16、基站布局要求高：uwb定位依赖于基站的布局，如果基站设置不合理或者数量不足，会导致定位盲区或精度下降。

17、成本较高：为了实现较高精度的定位，需要在井下布置大量的uwb基站，这增加了系统的成本和复杂性。

18、(2)ins定位技术的缺陷：

19、误差累积：ins是通过积分加速度和角速度数据来推算位置和姿态的，因此随着时间的推移，误差会逐渐累积，导致定位精度下降。

20、初始对准时间长：ins在使用前需要进行较长的初始对准过程，以确定初始位置和姿态，这限制了其在快速响应场景中的应用。

21、(3)激光传感器及ir(红外线)定位的缺陷：

22、受环境因素影响大：激光传感器及ir(红外线)对环境条件较为敏感，如湿度、粉尘等都可能影响其测量精度。

23、扫描范围有限：激光传感器的扫描范围通常受到限制，可能无法覆盖整个井下环境。

24、(4)camera和lidar定位的缺陷：

25、对光照条件敏感：camera定位在光照条件不佳时性能会大幅下降，而井下环境往往光照不足。

26、数据处理复杂：camera和lidar采集的数据量庞大，需要高性能的处理器进行实时处理，这在井下环境中可能是一个挑战。

27、成本较高：高精度的camera和lidar设备成本较高，可能增加系统的总体成本。

28、综上所述，现有的井下无人飞行器定位技术都存在各自的缺陷，无法满足高精度、高稳定性和低成本的综合需求。

技术实现思路

1、本发明要解决的技术问题是针对上述现有技术的不足，提供一种井下无人飞行器高精度融合定位系统及方法，结合了uwb(超宽带技术)、ins(惯性导航系统)、ir(红外线)、camera(摄像头)以及lidar(激光雷达)等多种传感器技术，以实现在没有卫星信号的井下环境中的高精度定位。

2、为解决上述技术问题，本发明所采取的技术方案是：一方面，本发明提供一种井下无人飞行器高精度融合定位系统，包括数据采集模块、通信模块、机载电脑和电源模块；所述数据采集模块包括uwb定位单元、ins单元、ir单元、图像采集单元和lidar单元；

3、所述uwb定位单元采用专用的uwb芯片组，采集无人飞行器与井下布置的uwb基站之间的距离；

4、所述ins单元包括高精度的陀螺仪和加速度计，用于测量无人飞行器的姿态和动态参数；

5、所述ir单元与ins单元结合用于测量无人飞行器的离地高度数据；

6、所述lidar单元和ins单元结合扫描局部巷道周边环境，形成局部三维扫描巷道地图；

7、所述图像采集单元采用高分辨率摄像头捕捉无人飞行器无人飞行器所在环境图像；

8、所述uwb定位单元、ins单元、ir单元、图像采集单元和lidar单元均与机载电脑连接，将采集的数据传输至机载电脑；

9、所述机载电脑接收并处理数据采集模块采集的多种类型数据，并运行定位算法进行融合定位，输出无人飞行器最终的定位结果；

10、所述通信模块用于实现机载电脑与地面控制中心之间的数据传输和通信。

11、所述电源模块与数据采集模块、通信模块和机载电脑均连接，提供供电电源。

12、另一方面，本发明还提供一种井下无人飞行器高精度融合定位方法，使用以下步骤进行迭代定位：

13、步骤1、建立无人飞行器状态方程；

14、已知无人飞行器上一时刻位置w(n-1)，速度通过ins单元采集无人飞行器当前时刻加速度数据根据运动方程计算当前位置w(n)和速度如下公式所示：：

15、

16、其中，δt为无人飞行器状态更新周期，与uwb定位更新周期相同，为0.5秒；

17、则建立无人飞行器状态方程为：

18、

19、其中，s(n)为无人飞行器状态值，协方差矩阵为：

20、p(n)＝ap(n-1)at+q

21、其中，∈1为过程噪声方差；

22、步骤2、建立无人飞行器测量方程；

23、无人飞行器测量方程如下公式所示：

24、

25、其中，为无人飞行器状态测量值，c＝[1 0]，r＝∈a，∈a为测量噪声方差，为无人飞行器的观测位置，xn,yn,zn为无人飞行器位观测三维坐标值，为观测速度；

26、步骤3、测量无人飞行器的观测速度和观测位置

27、步骤3.1：已知井下巷道3d模型及模型中各点三维位置坐标；通过ins单元采集无人飞行器当前时刻加速度数据及无人飞行器的姿态；

28、步骤3.2：图像采集单元使用摄像头获得图像数据，结合步骤3.1中ins单元采集到的无人飞行器姿态数据将图像旋转至水平方向，分析图像在前一时刻及当前时刻的帧数据，得到像素的移动速度，然后将像素的移动速度转换为无人飞行器在x轴和y轴方向上的观测速度及

29、步骤3.3：安装在无人飞行器底部ir单元向巷道地面发射光线脉冲，结合步骤3.1中ins单元采集到的无人飞行器姿态数据得到当前时刻离地高度，减去前一时刻算出的离地高度，高度差除以间隔时间，计算出无人飞行器在z轴方向上的观测速度

30、步骤3.4：得到无人飞行器观测速度

31、步骤3.5：uwb定位单元每秒二次测量无人飞行器与井下布置的uwb基站之间的距离，距离当前时刻最近的一次测量结果表明无人飞行器在基站a{x1,y1,z1})和b{x2,y2,z2}之间，且与基站a的测量距离为a，则无人飞行器当前时刻三维坐标估算为：

32、

33、步骤3.6：以步骤3.5估算的无人飞行器当前时刻三维坐标为圆心，以井下巷道宽度的最大值为半径从井下巷道3d模型截取局部巷道模型；

34、步骤3.7：利用lidar单元、imu单元进行局部巷道周边环境扫描，形成局部三维扫描巷道地图；

35、步骤3.8：对步骤3.7获取的局部三维扫描巷道地图的扫描点云进行关键点检测、特征描述符提取形成分辨率栅格地图，使用面元特征的三维栅格匹配算法将分辨率栅格地图与步骤3.6中得到的局部巷道模型进行匹配，通过异常值剔除和转换估计得到无人飞行器在井下局部巷道模型中位置信息，进而的到无人飞行器当前时刻的观测位置

36、步骤4、基于无人飞行器的观测速度和观测位置以及无人飞行器的状态方程和测量方程连续地估计和跟踪无人飞行器的位置信息及速度信息，实现无人飞行器的连续融合定位；

37、已知上一时刻无人飞行器状态最优估计值及协方差矩阵最优估计值用无人飞行器状态方程对当前时刻状态s(n)及协方差矩阵p(n)进行预测，如下公式所示：

38、

39、其中，分别为无人飞行器状态及协方差矩阵的预测值；

40、然后使用卡尔曼滤波算法通过无人飞行器状态测量值来修正预测值，得到无人飞行器状态和协方差矩阵最优估计值，如下公式所示：

41、

42、其中，和分别为当前时刻无人飞行器状态和协方差矩阵最优估计值，

43、本发明的井下无人飞行器高精度融合定位系统及方法的关键点主要包括以下几个方面：

44、(1)多传感器数据融合策略：

45、本发明采用了一种独特的多传感器数据融合策略，该策略能够高效地整合来自uwb、ins、ir(红外线)、camera和lidar等不同传感器的数据。这种融合策略不仅提高了数据的互补性和冗余性，还有效减少了单一传感器数据可能带来的误差和不确定性。

46、(2)基于uwb和ins的初步定位：

47、利用uwb技术提供的高精度测距信息，结合ins的姿态和动态数据，本发明实现了一种初步定位方法。这种定位方法在没有卫星信号的环境中尤为重要，它能够为后续的精确定位提供可靠的初始估计。

48、(3)ir(红外线)与lidar的环境感知校正：

49、ir(红外线)和lidar提供了丰富的环境感知信息，这些信息被用来校正初步定位的结果。本发明设计了一种校正算法，能够准确地利用这些传感器数据对初步定位结果进行微调，从而提高定位精度。

50、(4)基于camera的视觉辅助定位：

51、camera捕获的图像数据不仅用于环境识别，还通过图像匹配和特征提取技术，辅助无人飞行器进行更精确的定位。本发明中，视觉辅助定位作为一个关键点，显著提升了整个定位系统的准确性和鲁棒性。

52、(5)实时动态调整与优化：

53、本发明的定位方法具有实时动态调整的能力。系统能够根据不同传感器的实时数据，动态地调整定位策略，优化定位结果。这种实时性保证了无人飞行器在井下复杂多变的环境中能够持续获得准确的定位信息。

54、(6)系统鲁棒性与容错性设计：

55、考虑到井下环境的恶劣性和不确定性，本发明特别强调了系统的鲁棒性和容错性设计。通过合理的传感器布局、冗余设计以及错误检测和修复机制，确保定位系统在部分传感器失效时仍能维持一定的定位能力。

56、综上所述，本发明的关键点在于创新性地融合了多种传感器技术，通过初步定位、环境感知校正、视觉辅助定位、实时动态调整与优化以及系统鲁棒性设计等多个方面的综合应用，实现了在井下无卫星信号环境中的高精度无人飞行器定位。

57、采用上述技术方案所产生的有益效果在于：本发明提供的一种井下无人飞行器高精度融合定位系统及方法，通过过融合uwb、ins、ir(红外线)、camera和lidar等多种传感器技术，显著提高了在无卫星信号的井下环境中的定位精度和稳定性。与传统的定位方法相比，本发明具有以下突出优点和效果：

58、(1)高精度定位：通过融合多种传感器数据，本发明能够在井下环境中实现厘米级甚至毫米级的定位精度。实验数据显示，在相同的测试条件下，本发明的定位精度比单一使用uwb或ins的定位方法提高了至少35％，比传统的ir(红外线)或camera定位提高了50％以上。

59、(2)强抗干扰能力：井下环境常常伴随着粉尘、湿度、温度变化等多种干扰因素。本发明通过数据融合和算法优化，有效抵抗了这些干扰，保证了定位的稳定性和可靠性。。

60、(3)实时性：本发明的定位算法具有高效的计算性能，能够实现毫秒级的响应速度，满足井下无人飞行器快速移动时的实时定位需求。

61、(4)广泛的适用性：由于融合了多种传感器技术，本发明不仅适用于井下的复杂环境，还可以扩展到其他类似的无卫星信号或信号受限的环境中，如隧道、地下室等。这种广泛的适用性为无人飞行器的多样化应用提供了有力支持。

62、(5)鲁棒的系统设计：通过冗余设计和错误检测机制，本发明在部分传感器发生故障时仍能维持一定的定位精度，大大提高了系统的鲁棒性。在实际测试中，即使某个传感器出现故障，本发明的定位误差也仅增加了不到15％，远低于传统方法失效的风险。