一种基于位姿图的雷达点云地图更新和扩展方法及系统与流程

本申请公开一种基于位姿图的雷达点云地图更新和扩展方法及系统，涉及雷达定位。

背景技术：

1、随着自动驾驶领域的拓展，应用场景逐步丰富，在不同场景中建立的地图所需功能要求逐步增多。自动驾驶的功能实现基础之一是在已建立地图上的精准定位，而目前绝大部分自动驾驶的定位功能建立在事先建立的静态地图基础上，如常见的低速场景如园区、停车场等。由于以上场景中的车辆和行人流动性大，导致之前所建立的静态地图路况变化频繁，若不能及时对地图进行更新优化，会导致定位精准性大大降低，从而影响自动驾驶的整体功能，甚至会导致该场景中的自动驾驶功能失效。同时，在静态地图未涉及的区域无法进行定位，需要手动操控车辆通过。车辆行驶经过时如何对地图进行动态的扩展，保证下次经过时自动驾驶功能的可使用性，是需要解决的问题。

技术实现思路

1、本申请要解决的技术问题是车辆行驶经过路况已经发生变化的静态地图区域，如何对全局地图进行动态的扩展，保证下次经过时自动驾驶功能的可使用性。

2、为了解决上述技术问题，本申请的技术方案是提供了一种基于位姿图的雷达点云地图更新和扩展方法，包括：

3、步骤一 采集雷达的点云数据以及惯性传感器imu数据，对每一帧的雷达点云数据进行编码；

4、步骤二 根据雷达点云数据确定当前帧在全局地图中的定位位姿；

5、步骤三 采用iris回环检测确认当前帧在全局地图的对应帧和位姿图中的对应位姿；

6、步骤四 地图更新和扩展：根据当前帧雷达点云数据、定位位姿和全局地图的对应帧的点云数据、对应位姿，分三种情况处理；

7、步骤4.1 首先判断定位位姿和对应位姿之间的距离，当距离大于设定阈值时，说明车辆位于地图以外的未知区域和地图已建区域中间，通过同步定位与建图算法进行建图，对地图进行扩展；

8、步骤4.2 当距离小于设定阈值，进一步进行点云匹配度判断；获取点云之间的匹配精度；

9、步骤4.2-1 当匹配精度低于匹配阈值时，表示当前帧点云和全局地图的匹配程度较好，环境路况变化不大，则无需进行地图更新；

10、步骤4.2-2 若匹配精度高于匹配阈值时，表示当前帧点云和全局地图的匹配程度较差，代表当前环境路况变化较大，需要进行更新；用新的点云数据替换旧的点云数据，位姿图不进行更新。

11、优选的，所述步骤二具体包括：

12、步骤2.1 首先根据第0帧点云数据确定车辆在全局地图中的对应位姿；然后该对应位姿作为初始位姿，采用第1帧点云数据之前的imu数据在初始位姿上积分得到先验位姿；

13、步骤2.2 接着通过第1帧点云数据和全局地图进行匹配得到估计位姿，估计位姿结合先验位姿进行无迹卡尔曼处理，得到融合更新后的定位位姿；

14、步骤2.3 通过重复上述步骤，不断对新加入的imu和点云数据进行处理，得到输出频率为h的定位位姿；每一帧雷达点云数据对应一个定位位姿。

15、优选的，所述步骤三具体包括：全局地图是由多帧点云数据通过对应的全局位姿图转换到全局坐标系下，并采用点云相加的方式得到；在得到步骤二的定位位姿后，需要将当前定位位姿的点云数据通过回环检测确认全局地图的对应帧和位姿图中的对应位姿；iris回环通过将当前帧雷达点云数据转换为鸟瞰图图像，并和全局地图的鸟瞰图图像进行图像相似度对比来确认对应的地图点云。

16、优选的，所述步骤4.2中，点云之间的匹配精度通过两帧点云每个对应点之间的欧式距离除以点云数量来表示。

17、本申请还提供了一种基于位姿图的雷达点云地图更新和扩展系统，用于实现上述的基于位姿图的雷达点云地图更新和扩展方法，包括设于移动车辆上的雷达、惯性传感器、执行基于位姿图的雷达点云地图更新和扩展方法具体步骤的电子设备。

18、其中，所述电子设备包括一个或多个处理器以及配套的存储装置，存储装置内存储有执行基于位姿图的雷达点云地图更新和扩展方法具体步骤的程序，处理器从存储装置中读取程序并执行实现基于位姿图的雷达点云地图更新和扩展方法，处理器执行程序所需数据从雷达和惯性传感器获取。

19、本申请提供的基于位姿图的雷达点云地图更新和扩展方法和系统，通过iris回环检测、节点位姿距离判断、点云匹配精度比较来对全局地图进行更新，车辆运行到地图区域以外时，通过同步定位与建图算法对地图进行扩展。大大提高了点云地图的时效性和定位匹配的准确率，提高园区、停车场等低速场景道路上的定位精度和鲁棒性。因此可以解决道路情况变化较大，地图建立时间久远的问题。

技术特征：

1.一种基于位姿图的雷达点云地图更新和扩展方法，其特征在于，包括：

2.如权利要求1所述的一种基于位姿图的雷达点云地图更新和扩展方法，其特征在于，所述步骤二具体包括：

3.如权利要求1所述的一种基于位姿图的雷达点云地图更新和扩展方法，其特征在于，所述步骤三具体包括：全局地图是由多帧点云数据通过对应的全局位姿图转换到全局坐标系下，并采用点云相加的方式得到；在得到步骤二的定位位姿后，需要将当前定位位姿的点云数据通过回环检测确认全局地图的对应帧和位姿图中的对应位姿；iris回环通过将当前帧雷达点云数据转换为鸟瞰图图像，并和全局地图的鸟瞰图图像进行图像相似度对比来确认对应的地图点云。

4.如权利要求1所述的一种基于位姿图的雷达点云地图更新和扩展方法，其特征在于，所述步骤4.2中，点云之间的匹配精度通过两帧点云每个对应点之间的欧式距离除以点云数量来表示。

5.一种基于位姿图的雷达点云地图更新和扩展系统，其特征在于，用于实现权利要求1-4任一项所述的基于位姿图的雷达点云地图更新和扩展方法，包括设于移动车辆上的雷达、惯性传感器、执行基于位姿图的雷达点云地图更新和扩展方法具体步骤的电子设备。

6.如权利要求5所述的一种基于位姿图的雷达点云地图更新和扩展系统，其特征在于，所述电子设备包括一个或多个处理器以及配套的存储装置，存储装置内存储有执行基于位姿图的雷达点云地图更新和扩展方法具体步骤的程序，处理器从存储装置中读取程序并执行实现基于位姿图的雷达点云地图更新和扩展方法，处理器执行程序所需数据从雷达和惯性传感器获取。

技术总结
本申请公开了一种基于位姿图的雷达点云地图更新和扩展方法和系统，方法包括：步骤一 采集雷达的点云数据以及惯性传感器imu数据，对每一帧的雷达点云数据进行编码；步骤二 根据雷达点云数据确定当前帧在全局地图中的定位位姿；步骤三 采用Iris回环检测确认当前帧在全局地图的对应帧和位姿图中的对应位姿；步骤四 地图更新和扩展：根据当前帧雷达点云数据、定位位姿和全局地图的对应帧的点云数据、对应位姿，分三种情况进行地图扩展或更新。大大提高了点云地图的时效性和定位匹配的准确率，提高园区、停车场等低速场景道路上的定位精度和鲁棒性。可以解决道路情况变化较大，地图建立时间久远的问题。

技术研发人员：叶霆锋,陆新飞,薛旦,史颂华,赵映重,李家松,娄慧丽,潘松,张建君
受保护的技术使用者：上海几何伙伴智能驾驶有限公司
技术研发日：
技术公布日：2024/1/15