一种地下空间导航定位方法及导航定位系统与流程

本发明涉及地下空间导航定位，尤其涉及一种地下空间导航定位方法及导航定位系统。

背景技术：

1、由于地下空间无gps信号，且环境恶劣、沙尘多、湿度大等问题，常用的导航定位方式无法在井下使用。全站仪和激光雷达采用激光测距方法进行测量，测距精度可达到毫米级，但容易受结构等遮挡影响，需要专业测绘人员进行井下测量，无法保证实时性。目前，地下空间的导航定位方法多采用全站仪接力测量导线控制点的方式进行，但无法提供实时的高精度定位结果，且受地下空间的粉尘多、结构复杂等影响导致现场测绘测量工作复杂或难以进行高精度位置测量。

技术实现思路

1、为了解决现有技术存在的问题之一，本发明提供了一种地下空间导航定位方法及导航定位系统。

2、根据本发明的一方面，提供了一种地下空间导航定位方法，导航定位方法包括：

3、通过显控终端控制全站仪在地下空间进行自主设站；

4、利用全站仪自动追踪设置在移动目标上的定位棱镜以获取移动目标的第一位置信息；

5、利用激光雷达里程计获取移动目标的第一速度信息，利用里程计获取移动目标的第二速度信息，利用惯导装置解算得到移动目标的姿态信息、第三速度信息和第二位置信息，其中，激光雷达里程计、里程计和惯导装置均设置在移动目标上；

6、通过设置在移动目标上的信息处理器根据第一位置信息、第一速度信息、第二速度信息、姿态信息、第三速度信息和第二位置信息建立数据融合滤波器，并通过卡尔曼滤波得到移动目标的组合导航定位结果和导航精度自评价结果；

7、通过显控终端根据组合导航定位结果和导航精度自评价结果调整全站仪的光强和搜索强度以提高导航定位精度。

8、进一步地，姿态信息包括北天东向失准角，第一速度信息、第二速度信息和第三速度信息均包括北天东速度，第一位置信息和第二位置信息均包括经度、纬度和高度。

9、进一步地，组合导航定位结果包括移动目标的姿态、移动目标的速度和移动目标的位置。

10、进一步地，导航精度自评价结果包括经度误差、纬度误差和高度误差。

11、进一步地，数据融合滤波器的状态量为：

12、

13、上式中，x表示状态量，φn,φu,φe表示北天东向失准角，δvn,δvu,δve表示北天东速度误差，表示纬度误差，δh表示高度误差，δλ表示经度误差，εx,εy,εz表示载体系xyz向陀螺漂移，表示载体系xyz向加速度计零偏，δψlidar_odo表示激光雷达里程计的航向安装误差，δkodo表示里程计刻度因数误差，δtodo表示里程计时间延迟，δtlidar_odo表示激光雷达里程计时间延迟，δtets表示全站仪时间延迟。

14、进一步地，数据融合滤波器的观测量为：

15、

16、上式中，z表示观测量，表示惯导装置测得的纬度，表示全站仪测得的纬度，h表示惯导装置测得的高度，表示全站仪测得的高度，λ表示惯导装置测得的经度，表示全站仪测得的经度，vn表示惯导装置测得的北向速度，ve表示惯导装置测得的东向速度，表示里程计测得的北向速度，表示里程计测得的东向速度，和依次表示惯导装置载体系x轴向位移，和依次表示激光雷达里程计载体系x轴向位移、y轴向位移和z轴向位移。

17、进一步地，数据融合滤波器的量测矩阵为：

18、

19、其中，h1＝i3×3，

20、

21、上式中，h(t)表示量测矩阵，i3×3表示3×3的单位矩阵，vu表示惯导装置测得的天向速度，rm表示子午面主曲率半径，rn表示卯酉圈曲率半径，δvn表示惯导装置相邻两次测得的北向速度的差值，δve表示惯导装置相邻两次测得的东向速度的差值，表示姿态转移矩阵，表示姿态转移矩阵的逆。

22、根据本发明的另一方面，提供了一种地下空间导航定位系统，导航定位系统包括显控终端、全站仪、定位棱镜、激光雷达里程计、里程计、惯导装置和信息处理器，全站仪设置在地下空间，全站仪、定位棱镜、激光雷达里程计、里程计、惯导装置和信息处理器均设置在移动目标上；

23、显控终端用于控制全站仪在地下空间进行自主设站；

24、全站仪用于通过自动追踪定位棱镜以获取移动目标的第一位置信息；

25、激光雷达里程计用于获取移动目标的第一速度信息；

26、里程计用于获取移动目标的第二速度信息；

27、惯导装置用于解算得到移动目标的姿态信息、第三速度信息和第二位置信息；

28、信息处理器用于根据第一位置信息、第一速度信息、第二速度信息、姿态信息、第三速度信息和第二位置信息建立数据融合滤波器，并通过卡尔曼滤波得到移动目标的组合导航定位结果和导航精度自评价结果；

29、显控终端还用于根据组合导航定位结果和导航精度自评价结果调整全站仪的光强和搜索强度以提高导航定位精度。

30、应用本发明的技术方案，提供了一种地下空间导航定位方法及导航定位系统，该方法通过在移动目标上设置定位棱镜、激光雷达里程计、惯导装置、里程计和信息处理器，通过显控终端控制全站仪在地下空间进行自主设站，利用全站仪自动追踪定位棱镜以获取移动目标的第一位置信息，利用激光雷达里程计和里程计分别获取移动目标的第一速度信息和第二速度信息，利用惯导装置解算得到移动目标的第二位置信息、第三速度信息以及姿态信息，根据这些传感器测得的信息建立数据融合滤波器，并通过卡尔曼滤波得到移动目标的组合导航定位结果和导航精度自评价结果，同时，显控终端根据卡尔曼滤波结果调整全站仪的光强的搜索强度，进而提升导航定位精度。该方法能够实现矿井、隧道等地下空间的实时高精度定位和导航，导航定位精度能够达到厘米级，且不会受到地下空间粉尘、复杂结构等的影响。

技术特征：

1.一种地下空间导航定位方法，其特征在于，所述导航定位方法包括：

2.根据权利要求1所述的定位方法，其特征在于，所述姿态信息包括北天东向失准角，所述第一速度信息、所述第二速度信息和所述第三速度信息均包括北天东速度，所述第一位置信息和所述第二位置信息均包括经度、纬度和高度。

3.根据权利要求2所述的定位方法，其特征在于，所述组合导航定位结果包括所述移动目标的姿态、所述移动目标的速度和所述移动目标的位置。

4.根据权利要求3所述的定位方法，其特征在于，所述导航精度自评价结果包括经度误差、纬度误差和高度误差。

5.根据权利要求4所述的定位方法，其特征在于，所述数据融合滤波器的状态量为：

6.根据权利要求5所述的定位方法，其特征在于，所述数据融合滤波器的观测量为：

7.根据权利要求6所述的定位方法，其特征在于，所述数据融合滤波器的量测矩阵为：

8.一种地下空间导航定位系统，其特征在于，所述导航定位系统包括显控终端、全站仪、定位棱镜、激光雷达里程计、里程计、惯导装置和信息处理器，所述全站仪设置在地下空间，所述全站仪、所述定位棱镜、所述激光雷达里程计、所述里程计、所述惯导装置和所述信息处理器均设置在移动目标上；

技术总结
本发明提供了一种地下空间导航定位方法及导航定位系统，该导航定位方法包括：控制全站仪在地下空间进行自主设站；利用全站仪自动追踪设置在移动目标上的定位棱镜以获取第一位置信息；利用激光雷达里程计获取第一速度信息，利用里程计获取第二速度信息，利用惯导装置解算得到姿态信息、第三速度信息和第二位置信息；通过信息处理器根据第一位置信息、第一速度信息、第二速度信息、姿态信息、第三速度信息和第二位置信息建立数据融合滤波器，通过卡尔曼滤波得到移动目标的组合导航定位结果和导航精度自评价结果。与现有技术相比，本发明的技术方案能够解决现有技术中地下空间导航定位精度低、实时性差且易受地下空间复杂环境影响的技术问题。

技术研发人员：郭玉胜,管春洋,陈静,张东明,王迪,宋雅兰,曾强,孙雪丽,周东灵,张晓磊
受保护的技术使用者：北京自动化控制设备研究所
技术研发日：
技术公布日：2024/1/12