视觉惯性定位方法及装置与流程

本发明涉及定位，具体是一种视觉惯性定位方法及装置。

背景技术：

1、在机器人/汽车/自动驾驶行业，对机器人/汽车自身的实时定位通常会依赖事先建立的高精地图实现。一种常见的定位模式为激光雷达定位，即通过将当前时刻激光雷达获取的点云帧数据与高精地图匹配获取当前定位数据。这是一种高精度的全局定位模式，也是行业中最广泛应用的定位模式。它最主要的缺点为所需的激光雷达传感器成本高，并且在退化场景中(如长走廊、隧道、广场等)往往不能实现良好定位，这些问题限制了定位算法的大规模商业应用。为应对以上问题，该专利的目标为，在良好的场景中采用一种低成本的定位方案，提高产品的市场竞争力；在复杂场景中，增加一种定位功能，当激光定位失效时，这种定位模式能起到替换作用。

技术实现思路

1、针对上述现有技术中的不足，本发明提供一种视觉惯性定位方法及装置，可有效地实现降成本定位以及在部分雷达定位失效场景的定位。

2、为实现上述目的，本发明提供一种视觉惯性定位方法，包括如下步骤：

3、步骤1，基于上一帧视觉帧的位姿以及惯性数据，得到当前视觉帧的初始位姿；

4、步骤2，基于当前视觉帧与上一帧视觉帧进行视觉特征的帧-帧匹配，并基于帧-帧匹配结果对当前视觉帧的位姿进行第一次优化，得到优化位姿；

5、步骤3，基于当前视觉帧上提取的视觉特征点与当前视觉地图中的视觉地图点进行视觉特征的帧-地图匹配，并基于帧-地图匹配结果对当前视觉帧的位姿进行第二次优化，得到当前视觉帧的精确位姿并输出。

6、在其中一个实施例，步骤1中，所述初始位姿的获取过程为：

7、定义当前视觉帧的状态量x为：

8、

9、其中，为惯性坐标系到世界坐标系的旋转矩阵、平移矩阵，为惯性坐标系在世界坐标系的速度，bω、ba为惯性设备imu的角速度偏差、线加速度偏差，gw为世界坐标系的重力加速度，t为矩阵的转置；

10、基于惯性积分得到当前视觉帧的初始位姿，为：

11、

12、其中，i是第i次惯性数据采样的标号，xi+1为上一帧视觉帧与当前视觉帧采样时刻间的第i+1次惯性数据采样时的状态量，xi为上一帧视觉帧与当前视觉帧采样时刻间的第i次惯性数据采样时的状态量，δt为相邻两次惯性数据采样时刻之差，ui表示第i次imu采样的实时惯性数据，wi表示第i次惯性数据采样的噪声，表示惯性积分状态更新过程，f(xi,ui,wi)为关于i到i+1时刻状态量的变化率。

13、在其中一个实施例，步骤2中，所述基于当前视觉帧与上一帧视觉帧进行视觉特征的帧-帧匹配，具体为：

14、对上一帧视觉帧所有图层上的所有视觉特征点，在当前视觉帧的对应图层进行光流追踪，得到上一帧视觉帧上视觉特征点与当前帧视觉帧上视觉特征点的2d-2d匹配关系。

15、在其中一个实施例，步骤2中，所述基于帧-帧匹配结果对当前视觉帧的位姿进行第一次优化，具体为：

16、获取上一帧视觉帧上所有视觉特征点与视觉地图上视觉地图点的2d-3d匹配关系，定义其为2d-3d匹配关系#1；

17、基于所述2d-2d匹配关系与所述2d-3d匹配关系#1，得到当前帧视觉帧上视觉特征点与视觉地图上视觉地图点的2d-3d匹配关系，定义其为2d-3d匹配关系#2；

18、基于所述初始位姿，将2d-3d匹配关系#2中对应的视觉地图点投影到当前帧视觉帧上，并根据投影结果与2d-3d匹配关系#2得到各个视觉地图点的投影误差#1；

19、以各个视觉地图点投影误差#1的和最小为目标，对当前视觉帧的位姿进行优化，即得到所述优化位姿。

20、在其中一个实施例，步骤3中，在当前视觉帧提取视觉特征点的过程为：

21、定义当前视觉帧上光流追踪到的视觉特征点为视觉特征点#1；

22、在当前视觉帧上提取新的视觉特征点#2，并使每个视觉特征点#2与任一视觉特征点#1至少距离k个像素，以避免特征在像素平面上的距离过近。

23、在其中一个实施例，步骤3中，所述基于当前视觉帧上提取的视觉特征点与当前视觉地图中的视觉地图点进行视觉特征的帧-地图匹配，具体为：

24、构建当前视觉帧的局部视觉地图，并将局部视觉地图投影到当前视觉帧的各个图层上，并只保留在当前视觉帧fov范围内的视觉地图点，定义这一类视觉地图点为视觉地图点#1；

25、计算所有视觉特征点#2的描述子，定义为描述子#1；

26、对于任一视觉地图点#1，进行如下操作：

27、获取该视觉地图点#1被观测的所有历史视觉帧，计算当前视觉帧与所有历史视觉帧的夹角，并获取该视觉地图点#1在夹角最小的历史视觉帧中的描述子，定义为描述子#2；

28、获取该视觉地图点#1在当前视觉帧每一图层上的像素坐标，定义为像素坐标#1；

29、对于当前视觉帧的每一图层，计算在以对应像素坐标#1为中心的窗口范围内，所有视觉特征点#2的描述子#1与描述子#2的相似度，并定义相似度最小的描述子#1所对应的视觉特征点#2为视觉特征点#3；

30、判断视觉特征点#3对应的相似度是否小于阈值：

31、若是，将视觉特征点#3与该视觉地图点#1的匹配结果2d-3d匹配关系#3作为一组帧-地图匹配结果并输出，再进行下一视觉地图点#1的处理；

32、否则，直接进行下一视觉地图点#1的处理；

33、遍历所有视觉地图点#1，即完成帧-地图匹配。

34、在其中一个实施例，所述构建当前视觉帧的局部视觉地图，具体为：

35、定义2d-3d匹配关系#2对应的所有视觉地图点为视觉地图点#2；

36、在视觉地图中，提取所有能够观测任一视觉地图点#2的视觉帧，定义为视觉帧#1；

37、在视觉地图中，提取所有与任一视觉帧#1相邻的视觉帧，定义为视觉帧#2；

38、将所有视觉帧#1与所有视觉帧2共同组成局部地图帧，并将局部地图帧记录到的所有视觉地图点提取出来，定义为视觉地图点#3；

39、所有视觉地图点#3即为所述当前视觉帧的局部视觉地图。

40、在其中一个实施例，步骤3中，所述基于帧-地图匹配结果对当前视觉帧的位姿进行第二次优化，具体为：

41、基于所述优化位姿，将2d-3d匹配关系#3中对应的视觉地图点投影到当前帧视觉帧上，并根据投影结果与2d-3d匹配关系#3得到各个视觉地图点的投影误差#2；

42、以各个视觉地图点投影误差#2的和最小为目标，对当前视觉帧的位姿进行优化，即得到所述精确位姿。

43、在其中一个实施例，若当前视觉帧为第1帧，其定位过程为：

44、基于回环检测得到当前视觉帧的粗定位位姿；

45、在所述粗定位的基础上，基于3d-2d匹配得到当前视觉帧的精确位姿。

46、在其中一个实施例，所述基于回环检测得到当前视觉帧的粗定位位姿，具体为：

47、对当前视觉帧提取新的视觉特征点及其描述子，借助词袋寻找回环帧，寻找与当前视觉帧最像的m帧备选视觉帧；

48、若具有定位装置的当前位姿，则在m个备选视觉帧中选择与当前位姿最近的一个备选视觉帧作为回环帧，将该回环帧的位姿作为当前视觉帧的粗定位位姿；

49、若不具有定位装置的当前位姿，则基于词袋中3d视觉地图点、备选视觉帧2d视觉特征点以及当前视觉帧2d视觉特征点的对应关系，进行光束平差法求出各个备选视觉帧对应的当前视觉帧位姿，并用各个备选视觉帧对应的当前视觉帧位姿将视觉地图点投影到当前视觉帧上，计算投影点和匹配视觉特征点的平均像素距离，将m帧备选视觉帧中投影误差最小的那一帧作为回环帧，将该回环帧的位姿作为当前视觉帧的粗定位位姿。

50、在其中一个实施例，所述基于3d-2d匹配得到当前视觉帧的精确位姿，具体为：

51、在视觉地图中查询到回环帧的相邻帧，并将所有相邻帧与回环帧共同作为局部地图帧；

52、将局部地图帧关联的所有视觉地图点选择出来，作为局部地图；

53、在当前视觉帧提取新的视觉特征点及其描述子，对于局部地图中那些没有3d坐标-当前视觉帧2d像素坐标匹配关系的视觉地图点，依次投影到当前视觉帧上并获得相应的像素坐标#2；

54、在像素坐标#2的周围范围内，依次和当前视觉帧上视觉特征点的描述子匹配，最接近的视觉特征点即为该视觉地图点的2d匹配，从而获得所有3d-2d的匹配，再根据光束平差法得到当前视觉帧的准确定位。

55、为实现上述目的，本发明还提供一种视觉惯性定位装置，采用上述的方法进行定位，所述视觉惯性定位装置包括：

56、初始位姿计算单元，用于根据上一帧视觉帧的位姿以及惯性数据，得到当前视觉帧的初始位姿；

57、帧-帧匹配单元，用于根据当前视觉帧与上一帧视觉帧进行视觉特征的帧-帧匹配；

58、第一优化单元，用于根据帧-帧匹配结果对当前视觉帧的位姿进行第一次优化，得到优化位姿；

59、帧-地图匹配单元，用于根据当前视觉帧上提取的视觉特征点与当前视觉地图中的视觉地图点进行视觉特征的帧-地图匹配；

60、第二优化单元，用于根据帧-地图匹配结果对当前视觉帧的位姿进行第二次优化，得到当前视觉帧的精确位姿并输出。

61、在其中一个实施例，视觉惯性定位装置还包括：

62、初始化/重定位单元，用于获取定位初始时刻或重启动时刻的位姿。

63、为实现上述目的，本发明还提供一种终端设备，所述终端设备为计算机、无人车、无人机、无人驾驶设备或移动机器人，所述终端设备上设有：

64、存储器，用于存储程序；

65、处理器，用于执行所述存储器存储的所述程序，当所述程序被执行时，所述处理器用于执行如上述的方法的部分或全部步骤。

66、为实现上述目的，本发明还提供一种计算机可读存储介质，所述计算机可读存储介质中存储有计算机执行指令；所述计算机执行指令被处理器执行时用于实现如上述的方法的部分或全部步骤。

67、本发明具有如下有益技术效果：

68、1.本发明采用视觉定位替代激光定位，不仅成本更低，而且视觉定位依赖于环境纹理信息而不是结构化信息，因此在退化场景中也能实现良好性能；

69、2.本发明采用描述子信息实现3d路标-2d观测直接的信息关联，无需在环境中布置先验参照物(如二维码等)，节约了成本；

70、3.本发明采用视觉定位替代激光定位，因此不存在累积误差，定位结果具有全局精度；

71、4.本算法采用了惯性积分获取位姿初始值，并通过帧间匹配对位姿进行提升，帧-地图匹配实现高精定位，这种三级计算位姿方法，极大地提高了定位精度和鲁棒性，不易丢失；

72、5.本发明在优选方案中可通过其它定位结果实现快速恢复，也可自行快速恢复定位。

技术特征：

1.一种视觉惯性定位方法，其特征在于，包括如下步骤：

2.根据权利要求1所述的视觉惯性定位方法，其特征在于，步骤2中，所述基于当前视觉帧与上一帧视觉帧进行视觉特征的帧-帧匹配，具体为：

3.根据权利要求2所述的视觉惯性定位方法，其特征在于，步骤2中，所述基于帧-帧匹配结果对当前视觉帧的位姿进行第一次优化，具体为：

4.根据权利要求3所述的视觉惯性定位方法，其特征在于，步骤3中，在当前视觉帧提取视觉特征点的过程为：

5.根据权利要求4所述的视觉惯性定位方法，其特征在于，所述构建当前视觉帧的局部视觉地图，具体为：

6.根据权利要求4所述的视觉惯性定位方法，其特征在于，步骤3中，所述基于帧-地图匹配结果对当前视觉帧的位姿进行第二次优化，具体为：

7.根据权利要求1至6任一项所述的视觉惯性定位方法，其特征在于，若当前视觉帧为第1帧，其定位过程为：

8.根据权利要求7所述的视觉惯性定位方法，其特征在于，所述基于回环检测得到当前视觉帧的粗定位位姿，具体为：

9.根据权利要求8所述的视觉惯性定位方法，其特征在于，所述基于3d-2d匹配得到当前视觉帧的精确位姿，具体为：

10.一种视觉惯性定位装置，其特征在于，采用权利要求1至9任一项所述的方法进行定位，所述视觉惯性定位装置包括：

技术总结
本发明公开了一种视觉惯性定位方法及装置，该方法包括：基于上一帧视觉帧的位姿以及惯性数据，得到当前视觉帧的初始位姿；进行视觉特征的帧‑帧匹配，并基于帧‑帧匹配结果对当前视觉帧的位姿进行第一次优化，得到优化位姿；进行视觉特征的帧‑地图匹配，并基于帧‑地图匹配结果对当前视觉帧的位姿进行第二次优化，得到当前视觉帧的精确位姿并输出。本发明用于导航定位领域，可有效地实现降成本定位以及在部分雷达定位失效场景的定位，不仅成本更低，而且视觉定位依赖于环境纹理信息而不是结构化信息，因此在退化场景中也能实现良好性能。同时不存在累积误差，定位结果具有全局精度，且具有良好的鲁棒性。

技术研发人员：安向京,胡瑞琪,余辉亮
受保护的技术使用者：广州行深智能科技有限公司
技术研发日：
技术公布日：2024/9/23