一种极端场景下SLAM算法的高精度评价方法

一种极端场景下slam算法的高精度评价方法
技术领域
1.本发明涉及自动驾驶技术领域，特别涉及一种极端场景下slam算法的高精度评价方法。


背景技术：

2.自动驾驶车辆在所处的室内环境、地下停车场、矿山等极端场景下进行精准地感知定位是其能自主导航并移动、进而作业的重要基础功能。而自动驾驶车辆能有效实现定位导航的前提以及基本技术是同步定位与地图构建，也即slam(simultaneous localization and mapping)。目前应用在上述场景中的自动驾驶车辆大多使用嵌入式设备或者工控机等为主要逻辑控制、信息处理单元，并结合摄像头、3d和2d激光雷达、车轮编码器、惯性测量单元等传感器获取周边环境的特征信息和自动驾驶车辆本身的运动信息，将多源信息进行融合并实现自身的定位和环境地图的构建。同时为了评估slam算法的定位和建图精度，如何在一般场景下常用的gps、rtk信号缺失的情况下精准鲁棒地获取自动驾驶车辆在极端环境中的真实位置便尤为重要。
3.现有技术的不足之处在于，目前的室内、地下矿等场景定位真值获取方法存在一定的缺陷。目前常用的方法则是采用一些传统的已有的室内定位技术，例如采取红外定位技术、超宽带定位(uwb)、wifi定位技术、动作捕捉系统定位技术等，但是这些方法同样也存在很多问题，比如对于wifi定位，其总精度较高但是其指纹信息收集量大、易收其他信号干扰，其表现对于复杂的工况环境并不稳定鲁棒。对于超宽带(uwb)定位和动作捕捉系统，其穿透性强、功耗低，但是造价成本和使用成本比较高，同时在周边环境布置相应设备和走线工作量繁杂，对于空间较大的室内、地下环境会造成很大的使用成本。


技术实现要素：

4.本发明的目的克服现有技术存在的不足，为实现以上目的，采用一种极端场景下slam算法的高精度评价方法，以解决上述背景技术中提出的问题。
5.一种极端场景下slam算法的高精度评价方法，包括：
6.构建场景实际情况构建不同类型的坐标系；
7.利用测绘设备基于apriltag路标测量构建全局的真值地图；
8.通过传感器模块对预先布置的apriltag路标进行信息识别、特征提取和联合优化，得到路标和传感器模块的位姿；
9.根据待评估的slam算法的定位结果和转换到全局的真值地图的真值定位结果进行对齐和分析误差，并评价定位精度。
10.作为本发明的进一步的方案：所述坐标系包括待评估的传感器坐标系s、摄像头坐标系c、slam坐标系o，以及直角坐标系w。
11.作为本发明的进一步的方案：所述利用测绘设备基于apriltag路标测量构建全局的真值地图的具体步骤包括：
12.利用测绘设备在室内选取合适的位置对预先布置的apriltag路标中心进行测量；
13.获取路标相对于测绘设备坐标系下的三维几何位置点集，建立全局的路标地图，并作为真值和先验信息。
14.作为本发明的进一步的方案：所述通过传感器模块对预先布置的apriltag路标进行信息识别、特征提取和联合优化，得到路标和传感器模块的位姿的具体步骤包括：
15.首先在自动驾驶车辆的移动过程中，提取观测到的第i个apriltag路标li(i∈1,2
…
)并进行位姿测量，获取传感器模块和路标中心的相对位姿变换t
ij
∈se(3)，其中i，j表示传感器模块在第i个位姿处的时候观察到第j个路标点；
16.根据得到的apriltag路标提取路标角点，并获取路标角点的3d位姿pci，其中i表示第i个传感器模块；
17.根据提取到的路标角点利用2d-3d的pnp算法求解相机位姿xi和相机测量值m
l
，其中i表示第i个传感器模块，l表示传感器模块对于路标的第l个观测；
18.融合上述输出信息构建基于贝叶斯网络的因子图进行增量平滑和优化，获得最终的apriltag的位姿li和相机的位姿xi。
19.作为本发明的进一步的方案：所述根据待评估的slam算法的定位结果和转换到全局的真值地图的真值定位结果进行对齐和分析误差，并评价定位精度的具体步骤包括：
20.基于待评估的slam算法的传感器和摄像头设置共视物，并分别建立局部坐标系s和c，其中s为待评估的传感器坐标系，c为摄像头坐标系；
21.分别对共视物进行公共特征的提取，得到摄像头坐标系下的特征集合fc和待评估传感器坐标系下的特征集合fs；
22.根据两个特征集合fc和fs，利用优化的数据关联算法获取两个坐标系下的相对位姿t
sc
；
23.最后将待评估slam算法的定位结果转换到全局的真值地图坐标系下，与基于apriltag路标的摄像头定位真值对比并评价其定位精度。
24.与现有技术相比，本发明存在以下技术效果：
25.通过采用上述的技术方案，利用测绘设备对apriltag路标进行测量构建全局的真值地图。并通过自动驾驶车辆运行slam算法，摄取图像并对图像进行apriltag路标提取，估计路标和相机位姿的相对位姿，最后获取优化后的路标位姿和摄像头位姿。同时待评估的slam算法定位也同步运行，对两个系统构建的定位结果进行对齐并分析其误差，实现对slam系统定位算法的评估。解决了现有技术在复杂的工况环境并不稳定鲁棒的问题。提供了一种较为廉价、定位精度高、使用成本和难度较低的评估方法。
附图说明
26.下面结合附图，对本发明的具体实施方式进行详细描述：
27.图1为本技术公开的一些实施例的高精度评价方法的步骤示意图；
28.图2为本技术公开的一些实施例的算法流程框图；
29.图3为本技术公开的一些实施例的坐标系的示意图。
具体实施方式
30.下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
31.请参考图1和图2，本发明实施例中，一种极端场景下slam算法的高精度评价方法，包括：
32.s1、构建场景实际情况构建不同类型的坐标系；所述坐标系包括待评估的传感器坐标系s、摄像头坐标系c、slam坐标系o，以及直角坐标系w。
33.具体实施方式中，如图3所示，图示为本实施例的坐标系的示意图。该待评估的传感器坐标系s和摄像头坐标系c采用固连方式耦合并布置在移动车辆上，slam坐标系o根据待评估传感器的第一帧数据建立，全站仪通过测量环境中的apriltag标签建立起全站仪坐标系w下的真值地图。
34.s2、利用测绘设备基于apriltag路标测量构建全局的真值地图，具体步骤包括：
35.利用测绘设备在室内选取合适的位置对预先布置的apriltag路标中心进行测量；
36.获取路标相对于测绘设备坐标系下的三维几何位置点集，建立全局的路标地图，并作为真值和先验信息。
37.本实施例中，测绘设备选取全站仪，而传感器模块选取摄像头。需要说明的是，本实施例中的测绘设备能够用其他不同类型的测绘设备进行置换、替代。
38.具体实施方式为：
39.主要目的是建立基于apriltag路标的原始的全局的真值地图，用以作为全局参考。
40.利用全站仪在室内选取合适的位置对事先布置在周边环境的一系列apriltag路标中心进行测量，获取路标相对于全站仪坐标系系w下的三维几何位置(xw,yw,zw)点集，由此建立起全局的路标地图，并将其作为真值和先验信息。
41.s3、通过传感器模块对预先布置的apriltag路标进行信息识别、特征提取和联合优化，得到路标和传感器模块的位姿，具体步骤包括：
42.主要目的是，基于相对廉价的摄像头传感器，对事先布置在周边环境的apriltag路标进行信息识别和特征提取、以及联合优化，从而获取摄像头相对于路标的位置以及摄像头自身在全站仪坐标系下的准确定位位置。
43.具体实施方式中，首先利用携带摄像头的自动驾驶车辆从起点出发，算法前端根据事先全站仪给定的apriltag先验真值地图信息。在移动过程中，提取观测到的第i个apriltag路标li(∈1,2
…
)并进行位姿测量，获取传感器模块和路标中心的相对位姿变换t
ij
∈se(3)，其中i，j表示传感器模块在第i个位姿处的时候观察到第j个路标点；
44.根据得到的apriltag路标进一步提取路标角点，并获取路标角点的3d位姿pci，其中i表示第i个传感器模块；
45.根据提取到的路标角点利用2d-3d的pnp算法求解相机位姿xi和相机测量值m
l
，其中i表示第i个传感器模块，l表示传感器模块对于路标的第l个观测；
46.最后在后端融合上述输出信息构建基于贝叶斯网络的因子图进行增量平滑和优
化，获得最终的apriltag的位姿li和相机的位姿xi。
47.s4、根据待评估的slam算法的定位结果和转换到全局的真值地图的真值定位结果进行对齐和分析误差，并评价定位精度。
48.主要目的是，将定位结果与真值定位结果进行对齐，并评估两者精度。
49.具体步骤包括：
50.首先，根据待评估slam算法所使用的的传感器的类型，如激光雷达或者是摄像头等，基于待评估的slam算法的传感器和摄像头设置共视物，并分别建立局部坐标系s和c，其中s为待评估的传感器坐标系，c为摄像头坐标系；需要注意分辨出此处的待评估的传感器与上述的传感器模块的区别，传感器模块主要可以选取摄像头或者相机等设备，是用于主动评估的，而待评估的传感器是待评估的。
51.再分别对共视物进行公共特征的提取，得到摄像头坐标系下的特征集合fc和待评估传感器坐标系下的特征集合fs；
52.根据两个特征集合fc和fs，利用优化的数据关联算法获取两个坐标系下的相对位姿t
sc
；
53.最后将待评估slam算法的定位结果转换到全局的真值地图坐标系下，与基于apriltag路标的摄像头定位真值对比并评价其定位精度。
54.如图3所示，图示为本实施例中的算法流程图。首先利用全站仪对apriltag进行测量构建全局的地图真值。当自动驾驶汽车开始运行slam算法的时候，摄像头首先获取图像进行apriltag路标的提取并开始估计路标和相机位姿的相对位姿，其次对提取的apriltag再次提取其四个角点，然后对这些角点运用pnp算法求解获取摄像头的位姿以及摄像头对路标的测量，将这些值全部输入后端构建贝叶斯网络的因子图实行增量优化，最后获取优化后的路标位姿和摄像头位姿。与此同时待评估的slam算法定位也同步运行，并输出一系列自身传感器在slam坐标系下的位姿，然后将待评估slam算法的定位结果根据标定外参矩阵转换到摄像头的坐标系下，最终对这两个系统构建的定位结果进行对齐并分析其误差，实现对slam系统定位算法的评估。
55.有益效果：
56.本实施例中，实现仅仅依靠低成本的摄像头和一个全站仪进行gps、rtk信号缺失下的室内定位结果真值获取，与现有方法相比，不需要在周边环境部署额外的电子测量设备和布置设备所需要的走线，实现了评价场景的灵活性和自由性，大大减小了评价的成本，提升了该发明的通用性和可商用性。
57.而且，构建且统一了不同传感器的坐标系，结合全站仪和摄像头扩展了室内定位真值的获取方式，与其他利用视觉标签等获取定位真值的方法不同，利用全站仪的高精度特性对基于因子图优化的april slam进行良好的初始化和先验提供，实现了对待评估slam算法的合理评价，并可由此拓展到其他更高精度的slam算法结合全站仪进行定位评估。
58.尽管已经示出和描述了本发明的实施例，对于本领域的普通技术人员而言，可以理解在不脱离本发明的原理和精神的情况下可以对这些实施例进行多种变化、修改、替换和变型，本发明的范围由所附权利要求及其等同物限定，均应包含在本发明的保护范围之内。