一种面向大尺度地图的实时定位及建图方法

本发明涉及实时定位以及建图领域，尤其是涉及一种面向大尺度地图的实时定位及建图方法。

背景技术：

1、驱使机器人在未知环境中自动运行，一种常见的方法通过定位建图系统，在有了区域的局部地图之后，就可以对机器人的进行路径规划，实现高精度的自动运行。

2、传统的定位建图系统，存在以下问题：

3、(1)使用激光雷达作为传感器的建图系统往往成本高，难以广泛应用；

4、(2)无磁力计与gnss参与的建图系统不适用于长时间运行，否则会造成极大的累积漂移误差。

5、(3)使用rtk的定位建图方案精度高但是需要繁琐的设置，无法在一般应用上无法普及，同时该方案在gnss信号不佳的情况下将失效。

6、近年来对于定位建图系统的研究趋向于使用多传感器方案，通过不同传感器间的特性，达到优缺点互补的目的。但是，相比于单传感器，多传感器需要考虑传感器数据之间的同步和融合问题，且同步与融合方法将严重影响最终的定位精度。

技术实现思路

1、本发明的目的是为了提供一种面向大尺度地图的实时定位及建图方法，提高室外大尺度环境下的定位精度。

2、本发明的目的可以通过以下技术方案来实现：

3、一种面向大尺度地图的实时定位及建图方法，包括以下步骤：

4、s1：确定建图范围及行进路径，获取行进途中的图像、imu及gnss信息，并将gnss信息转化为经纬度、海拔高度的形式；

5、s2：将图像以及imu数据输入前端线程，对传感器状态进行初始化并计算帧间位姿，选取关键帧；

6、s3：将帧间位姿与关键帧输入后端优化线程，得到局部位姿信息；

7、s4：将局部位姿信息以及关键帧输入建图线程，剔除多余的地图云点以及关键帧，建立局部地图；

8、s5：将局部地图输入回环检测线程，判断当前图像和关键帧是否为同一种地图，若是则进行回环矫正，若不是，则将局部地图合并入全局地图，并进行gnss优化。

9、所述前端线程、后端优化线程和建图线程中对于图像和imu数据的处理基于orb-slam3系统实现。

10、所述s2包括以下步骤：

11、s21：对图像提取orb特征点；

12、s22：对接收到的imu数据进行预积分，并对预积分量与相机图像进行时间同步；

13、s23：加载gnss数据，并对gnss数据与图像进行时间同步；

14、s24：初始化视觉惯性系统；

15、s25：根据orb特征点和时间同步后的imu、gnss信息进行帧间位姿估计，并选取关键帧。

16、所述gnss数据的接收频率为20赫兹。

17、所述s23包括以下步骤：

18、s231：将当前gnss数据的海拔高度、经纬度信息转为笛卡尔坐标系数据；

19、s232：初始化gnss系统，若已完成初始化则跳过此步骤；

20、s233：对当前图像帧时间戳对应的gnss信息帧做同步处理。

21、所述将当前gnss数据的海拔高度、经纬度信息转为笛卡尔坐标系数据的计算方法为：

22、

23、其中，lat表示纬度，lon表示经度，alt表示海拔高度，s为地球椭球体的极扁率，

24、

25、其中a为地球椭球体的长半径。

26、所述初始化gnss系统具体为：将接收到的第一帧gnss数据转化为笛卡尔坐标系数据后的坐标对齐至原点，对其后接收到的数据采用原点偏移的方法确定坐标：

27、

28、其中，下标i代表帧数，下标0代表原点，x、y、z代表笛卡尔坐标系下初始的坐标，x、y、z代表偏移后的坐标。

29、所述对当前图像帧时间戳对应的gnss信息帧做同步处理具体为：根据当前图像的时间戳，选择相邻时间戳的两帧gnss数据，取所述两帧gnss数据的平均值作为新的gnss数据与图像时间戳同步，取平均值公式如下：

30、

31、其中x为转换后的笛卡尔坐标系下的点与第一帧的点进行偏移计算后得到的结果，下标i与j代表帧数，k代表计算得到的数据。

32、所述后端优化线程使用orb-slam3的vio图优化，通过g2o实现，其残差定义为：

33、

34、

35、

36、

37、其中r为旋转因子，v为速度因子，p为位置因子，上述皆为李群形式，下标为关键帧的帧数，r代表残差，不同下标对应不同物理量的残差，g为重力加速度，此优化有一个给定窗口大小，最终的优化目标为：

38、

39、ρhub为鲁棒核函数，其可以降低误匹配对系统定位的影响。

40、所述gnss优化采用因子图优化方式，并基于ceres实现，优化目标为：

41、

42、其中z代表位姿测量值，v上标代表来自里程计，g上标代表来自gnss，t下标代表帧数；

43、里程计因子如下：

44、

45、其中p为描述位置的物理量，q为描述姿态的物理量，上标w代表来自全局，上标l代表来自局部；

46、gnss因子如下：

47、

48、与现有技术相比，本发明具有以下有益效果：

49、1、本发明使用相机、imu、单个gnss设备，系统部署相对简单，且传感器成本低廉，易于广泛应用；

50、2、本发明使用改进的orb-slam3系统，使用视觉惯性系统估计帧间位姿的精确度相较于其他系统更为精确，同时通过融入gnss信息减少了原有的累积漂移误差对系统的影响，并且在运行效率上仍能保持实时；

51、3、本发明在乡村城市道路的数据集上进行实验，并与高精度rtk方案做对比，实验结果表明本发明的精确相较于不使用gnss的方法有了约54％的提升，有效提升了定位精度。

技术特征：

1.一种面向大尺度地图的实时定位及建图方法，其特征在于，包括以下步骤：

2.根据权利要求1所述的一种面向大尺度地图的实时定位及建图方法，其特征在于，所述前端线程、后端优化线程和建图线程中对于图像和imu数据的处理基于orb-slam3系统实现。

3.根据权利要求1所述的一种面向大尺度地图的实时定位及建图方法，其特征在于，所述s2包括以下步骤：

4.根据权利要求3所述的一种面向大尺度地图的实时定位及建图方法，其特征在于，所述gnss数据的接收频率为20赫兹。

5.根据权利要求3所述的一种面向大尺度地图的实时定位及建图方法，其特征在于，所述s23包括以下步骤：

6.根据权利要求5所述的一种面向大尺度地图的实时定位及建图方法，其特征在于，所述将当前gnss数据的海拔高度、经纬度信息转为笛卡尔坐标系数据的计算方法为：

7.根据权利要求5所述的一种面向大尺度地图的实时定位及建图方法，其特征在于，所述初始化gnss系统具体为：将接收到的第一帧gnss数据转化为笛卡尔坐标系数据后的坐标对齐至原点，对其后接收到的数据采用原点偏移的方法确定坐标：

8.根据权利要求5所述的一种面向大尺度地图的实时定位及建图方法，其特征在于，所述对当前图像帧时间戳对应的gnss信息帧做同步处理具体为：根据当前图像的时间戳，选择相邻时间戳的两帧gnss数据，取所述两帧gnss数据的平均值作为新的gnss数据与图像时间戳同步，取平均值公式如下：

9.根据权利要求1所述的一种面向大尺度地图的实时定位及建图方法，其特征在于，所述后端优化线程使用orb-slam3的vio图优化，通过g2o实现，其残差定义为：

10.根据权利要求1所述的一种面向大尺度地图的实时定位及建图方法，其特征在于，所述gnss优化采用因子图优化方式，并基于ceres实现，优化目标为：

技术总结
本发明涉及一种面向大尺度地图的实时定位及建图方法，包括以下步骤：确定建图范围及行进路径，获取行进途中的图像、IMU及GNSS信息，并将GNSS信息转化为经纬度、海拔高度的形式；将图像以及IMU数据输入前端线程，对传感器状态进行初始化并计算帧间位姿，选取关键帧；将帧间位姿与关键帧输入后端优化线程，得到局部位姿信息；将局部位姿信息以及关键帧输入建图线程，剔除多余的地图云点以及关键帧，建立局部地图；将局部地图输入回环检测线程，判断当前图像和关键帧是否为同一种地图，若是则进行回环矫正，若不是，则将局部地图合并入全局地图，并进行GNSS优化。与现有技术相比，本发明具有在室外大尺度环境下的定位精度更高等优点。

技术研发人员：姜晓燕,薛昆仑,方志军,高永彬
受保护的技术使用者：上海工程技术大学
技术研发日：
技术公布日：2024/1/16