基于高精度先验地图的工程应急救援车辆定位方法及系统

本申请涉及应急救援车辆定位，具体而言，涉及一种基于高精度先验地图的工程应急救援车辆定位方法及系统。

背景技术：

1、在我国自然灾害频发，因此需要大量应急救援车辆保障日常救灾工作，目前车辆的定位方式主要依赖于gnss系统、惯导、里程计，但是应急救援车辆的工作环境比较恶劣，在隧道、山区、城市道路等无信号或遮挡严重的场景下，gnss系统通常无法准确定位，单纯靠gnss定位不能够确定车辆的实时位置，会大大延误救援的进程。因此我们需要一种方法在一定时间内不依赖gnss信号也能够完成车辆精确的定位。

2、高精度地图是指高精度，地图信息精细化的地图，地图包含道路、行人、障碍物、树木、建筑物、车道信息、车道指示标志模型。车道指示标志模型包括了指示牌、车道线。车道信息包括道路的坡度、曲率、路面平整度等，高精度地图可以为车辆的定位提高精度。

3、但是，由于道路上的交通状况是动态变化的，因此无法保证高精度地图的实时性，只能通过定期更新方式来维护高精度地图的实时性，但是效率较低。

技术实现思路

1、为了解决由于道路上的交通状况是动态变化的，因此无法保证高精度地图的实时性，只能通过定期更新方式来维护高精度地图的实时性，但是效率较低的问题，本申请提供了一种基于高精度先验地图的工程应急救援车辆定位方法及系统。

2、本申请的实施例是这样实现的：

3、第一方面，本申请提供一种基于高精度先验地图的工程应急救援车辆定位方法及系统，包括：

4、通过实验平台的车载雷达和相机对当前实验环境进行扫描建成高精度点云地图；

5、基于车辆的初始位置及车载传感器的信息，得到当前时刻粗略的位姿信息；

6、根据历史车载gnss位置信息和高精度地图减少里程计的累计误差，并相互进行补偿，提高车辆的位姿精度；

7、基于ndt算法对所述高精度点云地图进行改进，以提高点云配准方面精度；

8、使用关键帧匹配策略，在基于ndt的算法将车辆当前位姿信息与生成的高精度点云地图进行配准，得到车辆在全局地图中的精确位姿信息。

9、在一种可能的实现方式中，所述通过实验平台的车载雷达和相机对当前实验环境进行扫描建成高精度点云地图，进一步包括：

10、使用实验平台的车载雷达扫描周围环境；

11、使用实验平台的多个相机每隔0.3s拍摄环境照片；

12、根据所述车载雷达扫描得到的周围环境生产高精度点云地图，并根据环境照片进行渲染上色。

13、在一种可能的实现方式中，在所述基于车辆的初始位置及车载传感器的信息，得到当前时刻粗略的位姿信息中；

14、所述相机和所述车载雷达的相对关系已经提前预设，所述相机拍摄照片和所述车载雷达扫描图像通过相对坐标关系来转换。

15、在一种可能的实现方式中，所述车辆初始位置为所设定的车辆定位初始点，轨迹的推算起始位置为车辆丢失gnss信号的时刻所对应的位置。

16、在一种可能的实现方式中，在所述根据历史车载gnss位置信息和高精度地图减少里程计的累计误差，并相互进行补偿，提高车辆的位姿精度中；

17、所述里程计的短期位姿精度要高于gnss，而gnss的长期精度误差小于里程计；

18、当车辆在运动过程中丢失gnss信号时，通过丢失时刻车辆的位置作为推算初始位置，同时通过车载里程计来补偿车辆运动的短期误差。

19、在一种可能的实现方式中，所述基于ndt算法对所述高精度点云地图进行改进，以提高点云配准方面精度，进一步包括：

20、在ndt算法中，将参考帧点云地图分为一个个小方格；

21、计算下一时刻目标帧得到点云，变换到参考坐标；

22、计算落到对应参考帧小方格中的概率。

23、在一种可能的实现方式中，将单元格概率地图通过距离传感器远近而对单元格点云进行加权赋值，代替ndt算法的等量加权策略；

24、距离越远，点云产生畸变越大，赋予单元格权重就越小，距离越近，赋予单元格权重就越大。

25、在一种可能的实现方式中，所述使用关键帧匹配策略，具体包括：

26、假设在n时刻选取了一张关键帧，在n+t(t≥1)时刻将扫描帧与当前关键帧进行匹配，当前帧和上一个关键帧的位姿变化超过了一定的阈值，就将当前帧设置为一个新的关键帧，而n时刻的关键帧则被舍弃。

27、在一种可能的实现方式中，在所述通过实验平台的车载雷达和相机对当前实验环境进行扫描建成高精度点云地图，还包括：

28、根据平台生成的高精度点云地图进行裁剪、选中、渲染、上色一系列操作，生成可直接使用的先验地图。

29、第二方面，本申请提供一种基于高精度点云地图的工程应急救援车辆定位系统，包括：

30、高精度点云地图构建模块，用于通过实验平台的车载雷达和相机对当前实验环境进行扫描建成高精度点云地图；

31、当前位姿获取模块，用于基于车辆的初始位置及车载传感器的信息，得到当前时刻粗略的位姿信息；

32、误差补偿模块，用于根据历史车载gnss位置信息和高精度地图减少里程计的累计误差，并相互进行补偿，提高车辆的位姿精度；

33、配准改进模块，用于基于ndt算法对所述高精度点云地图进行改进，以提高点云配准方面精度；

34、精确位姿获取模块，用于使用关键帧匹配策略，在基于ndt的算法将车辆当前位姿信息与生成的高精度点云地图进行配准，得到车辆在全局地图中的精确位姿信息。

35、本申请提供的技术方案至少可以达到以下有益效果：

36、本申请提供的基于高精度点云地图的工程应急救援车辆定位方法几系统，通过实验平台将环境地图扫描完成之后生成高精度点云地图，相比较传统的单一使用轮式里程计或者gnss进行定位的精度更好；可以获取到车辆的全局地图定位信息；高精度地图在车辆定位过程中使用不依赖gnss信号，在各种工作环境下的定位性能鲁棒性更好，通过我们手工操作后的先验地图信息区分更明显，边界更清晰，有效降低了点云配准时间，更易于车辆定位。

技术特征：

1.一种基于高精度先验地图的工程应急救援车辆定位方法，其特征在于，包括：

2.如权利要求1所述的基于高精度点云地图的工程应急救援车辆定位方法，其特征在于，所述通过实验平台的车载雷达和相机对当前实验环境进行扫描建成高精度点云地图，进一步包括：

3.如权利要求1所述的基于高精度点云地图的工程应急救援车辆定位方法，其特征在于，在所述基于车辆的初始位置及车载传感器的信息，得到当前时刻粗略的位姿信息中；

4.如权利要求3所述的基于高精度点云地图的工程应急救援车辆定位方法，其特征在于，所述车辆初始位置为所设定的车辆定位初始点，轨迹的推算起始位置为车辆丢失gnss信号的时刻所对应的位置。

5.如权利要求1所述的基于高精度点云地图的工程应急救援车辆定位方法，其特征在于，在所述根据历史车载gnss位置信息和高精度地图减少里程计的累计误差，并相互进行补偿，提高车辆的位姿精度中；

6.如权利要求1所述的基于高精度点云地图的工程应急救援车辆定位方法，其特征在于，所述基于ndt算法对所述高精度点云地图进行改进，以提高点云配准方面精度，进一步包括：

7.如权利要求6所述的基于高精度点云地图的工程应急救援车辆定位方法，其特征在于，将单元格概率地图通过距离传感器远近而对单元格点云进行加权赋值，代替ndt算法的等量加权策略；

8.如权利要求1所述的基于高精度点云地图的工程应急救援车辆定位方法，其特征在于，所述使用关键帧匹配策略，具体包括：

9.如权利要求1所述的基于高精度点云地图的工程应急救援车辆定位方法，其特征在于，在所述通过实验平台的车载雷达和相机对当前实验环境进行扫描建成高精度点云地图，还包括：

10.一种基于高精度先验地图的工程应急救援车辆定位系统，其特征在于，包括：

技术总结
本申请涉及应急救援车辆定位技术领域，具体而言，涉及一种基于高精度先验地图的工程应急救援车辆定位方法及系统，一定程度上可以解决由于无法保证高精度地图的实时性，通过定期更新方式来维护高精度地图的实时性效率较低的问题。该方法包括：通过实验平台进行扫描建成高精度点云地图；基于车辆的初始位置及车载传感器信息，得到当前时刻粗略的位姿信息；根据历史车载GNSS位置信息和高精度地图减少里程计的累计误差，并相互进行补偿，提高车辆的位姿精度；基于NDT算法对高精度点云地图进行改进，以提高点云配准方面精度；使用关键帧匹配策略，在基于NDT的算法将车辆当前位姿信息与生成的高精度点云地图进行配准，得到车辆在全局地图中的精确位姿信息。

技术研发人员：倪涛,王瑞,冀雯晶,唐积文,宁青川
受保护的技术使用者：燕山大学
技术研发日：
技术公布日：2024/3/24