CN115435790A

CN115435790A - 一种视觉定位与视觉里程计位姿融合的方法及系统

Info

Publication number: CN115435790A
Application number: CN202211096493.4A
Authority: CN
Inventors: 顾升宇; 王强; 张小军
Original assignee: Visionstar Information Technology Shanghai Co ltd
Current assignee: Visionstar Information Technology Shanghai Co ltd
Priority date: 2022-09-06
Filing date: 2022-09-06
Publication date: 2022-12-06

Abstract

一种视觉定位与视觉里程计位姿融合的方法及系统，依靠设备端运行的实时高帧率视觉里程计和视觉定位位姿，通过RANSAC的Sim3估计剔除定位误差大的位姿。通过对转换矩阵参数的松耦合求解，获取稳定的视觉里程计到视觉定位的相似变换。通过线性插值将稳定的相似变换转化为平滑的相似变换，避免了位姿的跳动，将高帧率的视觉里程计通过平滑的相似变换实时转换到视觉定位坐标系获取与视觉里程计同帧率的位姿。通过本发明的视觉定位与视觉里程计位姿融合的方法及系统获得的位姿与直接定位获取的位姿相比，精度更高、稳定性更好、帧率更高，具有较高的实用价值。

Description

一种视觉定位与视觉里程计位姿融合的方法及系统

技术领域

本发明属于计算机视觉领域，具体涉及一种视觉定位与视觉里程计位姿融合的方法及系统。

背景技术

基于视觉的空间定位技术是增强现实、机器人、无人驾驶等领域的关键技术之一，是利用图像自动辨识周围环境、视觉定位等技术的核心，例如Google推出的VPS，华为推出的Cyberverse，视+推出的SpatialMap，EasyAR发布的Mega等。在AR应用里，常常利用摄像头拍摄，通过识别环境特征点，计算出当前设备的位姿，常见的有平面图片跟踪，3D模型跟踪和视觉定位(VPS)。

视觉定位为使用相机获取图像，提取图像的特征并与已知的特征匹配，在空间中计算出相机在地图或者已知坐标系的位置和姿态，视觉定位包括图像跟踪，3D模型跟踪，视觉定位VPS等。在AR领域，图像跟踪、3D模型跟踪、视觉定位VPS是一个耗费资源的计算过程，且受到用户操作方式和环境变化等因素，会出现跟踪失败、跟踪抖动、定位错误、定位偏差大、帧率低等一系列问题。

视觉里程计为通过连续图像序列计算出相机的相对位置和姿态，包括VO、VIO、SLAM、纯惯导里程计等。

一些AR设备在本地运行一个里程计(VO/VIO/SLAM)，由于视觉位姿定位为低帧率(例如1秒一帧定位)，而里程计为高帧率(30fps以上)的位姿定位，通过里程计的辅助，将里程计位姿融合到视觉定位位姿坐标系，从而实现高帧率流畅跟踪和定位的主观体验。目前有一些AR SDK实现了视觉定位(图像跟踪、3D模型跟踪、视觉定位VPS)与里程计(VO/VIO/SLAM/纯惯导里程计)的位姿融合，采用图像跟踪+里程计、3D模型跟踪+里程计、视觉定位VPS+里程计等方式，例如Vufario，EasyAR，Huawei AREngine。目前视觉定位和视觉里程计位姿融合一般采用如下步骤：

1、单帧定位计算出转换关系

通过单帧定位结果计算视觉里程计的位姿到视觉定位的位姿的转换关系；由于单帧精度的不足而出现不稳定误差，转换关系的反复变化导致位姿明显跳动，主观体验差。

2、连续单帧定位的结果做平滑

连续N个单帧定位结果计算出N个转换关系然后做转换关系的平均值或中值；连续单帧定位的结果做平滑，可以缓解位姿跳动的问题，但对于错误定位的鲁棒性不足，当错误定位出现多个的时候，很难全部剔除。

3、多帧定位

连续多帧一起做视觉定位联合优化求解坐标转换的结果；多帧联合定位可以缓解位姿跳动，以及错误定位的问题，但对于数据传输量，计算量和算法复杂度带来了压力，例如VPS这种上万次/秒的云定位请求，是一种高成本的压力。

此外，现有技术还存在以下问题：

1、在弱纹理、相似纹理、对称性物体、相同规格的场景下会出现视觉识别错误、识别偏差和识别失败的情况；

2、在挑战的角度和位置，遮挡或者操作方式的不友好的情况下出现视觉识别跳动、抖动、误差或者失败的情况，影响使用者的主观感受；

3、由于视觉尺度和终端里程计(VO/VIO/SLAM)尺度存在差异时，终端使用里程计转换到视觉坐标系时存在误差，定位的位姿不准；

4、当视觉定位出现较大变化时，位姿突变导致观察到的虚拟物体跳动，影响主观感受。

综上所述，由于视觉定位(图像跟踪，3D模型跟踪，视觉定位VPS)存在误差，且视觉定位(图像跟踪，3D模型跟踪，视觉定位VPS)的坐标系和本地里程计会存在一定程度的差异，给位姿融合造成一定的难度，会出现跳动、抖动、偏移的问题。如何实现视觉定位(图像跟踪，3D模型跟踪，视觉定位VPS)与里程计(VO/VIO/SLAM/纯惯导里程计)位姿高精度融合是一个亟待解决的问题。

发明内容

为了克服现有技术缺陷，本发明的一个目的是提供一种视觉定位与视觉里程计位姿融合的方法，本发明的另一个目的是提供一种视觉定位与视觉里程计位姿融合的系统。为了实现上述目的，本发明采用的技术方案如下：

本发明的一个方面提供一种视觉定位与视觉里程计位姿融合的方法，包括：

步骤1：获取当前时刻图像的里程计位姿和视觉定位位姿，所述里程计位姿与所述视觉定位位姿组成融合帧；

步骤2：保存最近时间内固定数量的所述融合帧，所述固定数量大于三个；

步骤3：通过所述固定数量的融合帧求解所述视觉里程计位姿与所述视觉定位位姿的相似变换的尺度参数和固定数量的转换矩阵；

步骤4：对所述固定数量的转换矩阵的参数进行松耦合求解得到稳定的相似变换；

步骤5：将所述稳定的相似变换进行平滑转换得到平滑的相似变换，根据所述平滑的相似变换将所述视觉里程计位姿转换至所述视觉定位位姿坐标系。

步骤6：输出与视觉里程计同帧率的视觉定位位姿。

优选地，所述视觉里程计位姿与所述视觉定位位姿均包括6Dof的位姿，所述融合帧包括时间戳、图像的视觉里程计位姿与视觉定位位姿。

优选地，保存最近时间内固定数量的所述融合帧，包括：

持续将当前时刻融合帧存入滑动窗口；

当所述滑动窗口的融合帧数量大于所述固定数量时，删除时间戳最早的融合帧。

优选地，通过所述固定数量的融合帧求解所述视觉里程计位姿与所述视觉定位位姿的相似变换的尺度参数和固定数量的转换矩阵，包括：

读取当前时刻所述滑动窗口内的固定数量的融合帧；

通过RANSAC的Sim3算法将所述融合帧中的视觉里程计位姿对齐至视觉定位位姿的坐标系；

剔除所述视觉定位位姿中的外点，得到所述视觉定位位姿中的内点，所述视觉定位位姿中的内点与对应的视觉里程计位姿中的三维点组成内点位姿对；

通过所述内点位姿对计算当前时刻所述视觉里程计位姿至所述视觉定位位姿的相似变换的尺度参数和固定数量的转换矩阵；

所述当前时刻固定数量的转换矩阵中每个转换矩阵的参数包括旋转矩阵和平移矩阵，所述相似变换的参数包括尺度参数、旋转矩阵和平移矩阵。

优选地，对所述转换矩阵的参数进行松耦合求解得到稳定的相似变换，包括：

计算所述固定数量的转换矩阵的旋转矩阵的中值与平移矩阵的中值；

将所述固定数量的转换矩阵中的所有旋转矩阵与所述旋转矩阵的中值比较，将所述固定数量的转换矩阵中的所有平移矩阵与平移矩阵的中值比较；

剔除所述旋转矩阵与所述旋转矩阵的中值差的绝对值大于阈值或者平移矩阵与平移矩阵的中值差的绝对值大于阈值的旋转矩阵，得到更新的转换矩阵；

通过所述更新的转换矩阵对应的三维点对的位姿、所述相似变换的尺度参数和所述旋转矩阵的中值计算所述相似变换的平移矩阵，得到稳定的相似变换。

优选地，将所述稳定的相似变换进行平滑转换得到平滑的相似变换，包括：

保留上一时刻的稳定的相似变换与当前时刻的稳定的相似变换；

通过插值法分别对所述上一时刻的稳定的相似变换与当前时刻的稳定的相似变换的尺度参数、旋转矩阵和平移矩阵进行计算，得到平滑的相似变换。

本发明的另一个方面提供一种视觉定位与视觉里程计位姿融合的系统，所述系统包括视觉定位模块、视觉里程计模块、融合模块、滑动窗口模块、位姿对齐模块、求解模块、坐标系转换模块、输出位姿模块，其中：

所述视觉定位模块用于获取当前时刻图像的视觉定位位姿；

所述视觉里程计模块用于获取当前时刻图像的视觉里程计位姿；

所述融合模块用于将所述视觉里程计位姿与所述视觉定位位姿组成融合帧；

所述滑动窗口模块用于保存最近时间内固定数量的所述融合帧，所述固定数量大于三个；

所述位姿对齐模块用于通过所述固定数量的融合帧求解所述视觉里程计位姿与所述视觉定位位姿的相似变换的尺度参数和固定数量的转换矩阵；

所述求解模块用于对所述固定数量的转换矩阵进行松耦合求解得到稳定的相似变换；

所述坐标系转换模块用于将所述稳定的相似变换进行平滑转换得到平滑的相似变换；

所述输出位姿模块用于根据所述平滑的相似变换将所述视觉里程计位姿转换至所述视觉定位位姿坐标系。

优选地，所述视觉定位模块、视觉里程计模块分别获取当前图像的视觉定位的6Dof的位姿、视觉里程计的6Dof的位姿，所述融合模块获取当前时间戳，所述融合帧包括所述时间戳、图像的视觉里程计位姿与视觉定位位姿。

优选地，所述滑动窗口模块持续存储当前时刻融合帧，当所述滑动窗口模块的融合帧数量大于所述固定数量时，删除时间戳最早的融合帧。

优选地，所述位姿对齐模块通过RANSAC的Sim3算法将所述滑动窗口模块中融合帧中的视觉里程计位姿对齐至视觉定位位姿的坐标系，剔除所述视觉定位位姿中的外点后，得到所述视觉定位位姿中的内点，所述视觉定位位姿中的内点与对应的视觉里程计位姿中的三维点组成内点位姿对，通过所述内点位姿对计算当前时刻所述视觉里程计位姿至所述视觉定位位姿的相似变换的尺度参数和固定数量的转换矩阵，所述当前时刻的相似变换的参数包括尺度参数、旋转矩阵和平移矩阵，所述固定数量的转换矩阵的参数包括旋转矩阵和平移矩阵。

优选地，所述求解模块剔除所述转换矩阵中所述旋转矩阵与所述旋转矩阵的中值差的绝对值大于阈值或者平移矩阵与平移矩阵的中值差的绝对值大于阈值的转换矩阵得到更新的转换矩阵，通过更新的转换矩阵对应的三维点对的位姿、所述相似变换的尺度参数和所述旋转矩阵的中值计算所述相似变换的平移矩阵，得到稳定的相似变换。

优选地，所述坐标系转换模块通过插值法分别对上一时刻的稳定的相似变换与当前时刻的稳定的相似变换的尺度参数、旋转矩阵和平移矩阵进行计算，得到平滑的相似变换。

本发明的再一个方面提供一种视觉定位与视觉里程计位姿融合的系统，包括存储器和处理器，其中：

所述存储器，用于存储代码和相关数据；

所述处理器，用于调用所述存储器中的数据，执行所述存储器中的代码能实施如前任一所述的方法和步骤。

本发明的视觉定位与视觉里程计位姿融合的方法及系统，依靠设备端运行的实时高帧率视觉里程计和视觉定位位姿，通过RANSAC的sim3估计剔除定位误差大的位姿。通过对转换矩阵参数的松耦合求解，获取稳定的视觉里程计到视觉定位的相似变换。通过线性插值将稳定的相似变换转化为平滑的相似变换，避免了位姿的跳动，将高帧率的视觉里程计通过平滑的相似变换实时转换到视觉定位坐标系获取与视觉里程计同帧率的位姿。通过本发明的视觉定位与视觉里程计位姿融合的方法及系统获得的位姿与直接定位获取的位姿相比，精度更高、稳定性更好、帧率更高，具有较高的实用价值。

附图说明

读者在参照附图阅读了本发明的具体实施方式以后，将会更加清楚地了解本发明的各个方面。其中，

图1为本发明的一个实施例的视觉定位与视觉里程计位姿融合方法的总体流程图；

图2为本发明的一个实施例的视觉定位与视觉里程计位姿融合系统的系统结构图；

图3为本发明的另一个实施例的视觉定位与视觉里程计位姿融合系统的系统结构图。

附图标记：

11：视觉定位模块；12：视觉里程计模块；13：融合模块；14：滑动窗口模块；

15：位姿对齐模块；16：求解模块；17：坐标系转换模块；18：输出位姿模块。

21：存储器；22：处理器。

具体实施方式

为了使本申请所揭示的技术内容更加详尽与完备，可参照附图以及本发明的下述具体实施例。然而，本技术领域的普通技术人员应当理解，下文中所提供的实施例并非用来限制本发明所涵盖的范围。此外，附图仅仅用于示意性地加以说明，并未依照其原尺寸进行绘制。

下面参照附图，对本发明各个方面的具体实施方式作进一步的详细描述。

实施例一：

本实施例提供一种视觉定位与视觉里程计位姿融合的方法，请参见图1，所述方法包括：

S1：获取当前时刻图像的里程计位姿和视觉定位位姿，所述里程计位姿与所述视觉定位位姿组成融合帧。在本实施例中，依靠设备端运行的实时高帧率视觉里程计/里程计/纯惯导以及在视觉定位获取当前时刻图像的视觉里程计与视觉定位的位姿，所述视觉里程计位姿与所述视觉定位位姿均包括6Dof的位姿，所述融合帧包括时间戳、图像的视觉里程计位姿与视觉定位位姿。具体地，获取连续N帧本地视觉里程计的位姿P1(P1包含了N个的6Dof位姿[R|t])，获取连续M帧的视觉定位的位姿P2(P2包含了M个的6Dof位姿[R|t]，所述融合帧fusionFrame包括time1时刻的时间戳、图像的视觉里程计位姿P1(6Dof位姿[R|t])与视觉定位位姿P2(6Dof位姿[R|t])。

S2：保存最近时间内固定数量的所述融合帧，所述固定数量大于三个，包括：

S21：持续将当前时刻融合帧存入滑动窗口；

S22：当所述滑动窗口的融合帧数量大于所述固定数量时，删除时间戳最早的融合帧。

S3：通过所述固定数量的融合帧求解所述视觉里程计位姿与所述视觉定位位姿的相似变换的尺度参数和固定数量的转换矩阵，包括：

S31：读取当前时刻所述滑动窗口内的固定数量的融合帧；

S32：通过RANSAC的Sim3算法将所述融合帧fusionFrame中的视觉里程计位姿P1对齐至视觉定位位姿P2的坐标系，剔除所述视觉定位位姿P2中的外点，得到所述视觉定位位姿中的内点P2i(P2i包含了X个的6Dof位姿[R|t])，所述视觉定位位姿中的内点P2i与对应的视觉里程计位姿中的三维点组成内点位姿对。

具体地，在本实施例中，采用RANSAC(Random Sample Consensus，随机采样一致)算法剔除所述外点。RANSAC算法是从一组含有“外点”(outliers)的数据中正确估计数学模型参数的迭代算法。“外点”一般指的是数据中的噪声，例如匹配中的误匹配和估计曲线中的离群点。因此RANSAC也是一种“外点”检测算法。具体地，包括以下步骤：

S321：随机从所述滑动窗口中采样X个融合帧fusionFrame(包含P1位姿和P2位姿，即X个位姿对)；

S322：对所述X个位姿对拟合Sim3模型；

S323：计算所述滑动窗口中的其他融合帧到所述拟合Sim3模型的距离，如果所述距离小于一定阈值，判断该点为内点，统计内点个数；

S324：重复执行M次步骤S321-323，选择内点数最多的Sim3模型作为最佳结果；

S325：利用所述S324输出的内点重新估计Sim3模型。具体地，由于步骤S324所输出的Sim3模型对应的为从所述滑动窗口中随机采样的融合帧，既包括内点，又包括外点，因此需要利用所述S324输出的内点重新拟合Sim3模型，以进一步提高精度；

S33：通过所述内点位姿对和S325中重新拟合的Sim3模型计算当前时刻所述视觉里程计位姿至所述视觉定位位姿的相似变换和固定数量的转换矩阵；

具体地，步骤S32中求解Sim3变换的过程，可以理解为求解两个坐标系的相似变换。具体地，所述当前时刻的相似变换参数包括尺度参数s、旋转矩阵R和平移矩阵t；具体地，所述S321中采样的X个融合帧fusionFrame中，X个P1位姿的相机位置为p1(t10，t11，t12，......)，其中t10，t11，t12，......等为三维坐标点，同时刻的X个P2位姿的相机位置为p2(t20，t21，t22，......)，其中t20，t21，t22，......等为三维坐标点。X个定位数据组成了X个三维点对，其中p2＝sR(p1)+t，在S32中通过RANSAC剔除外点后，只需要超过三组三维点对即可通过Sim算法求解出所述视觉里程计坐标系至所述视觉定位坐标系的相似变换的尺度参数s、3*3旋转矩阵R和3*1平移矩阵t，此为所述滑动窗口保存三个以上融合帧的原因。其中，P1中X个6Dof位姿与对应的P2i中X个6Dof位姿组成所述内点位姿对，通过所述P1中X个6Dof位姿与对应的P2i中X个6Dof位姿拟合生成所述Sim3模型，计算出X个6Dof转换矩阵Tx(Tx包含了X个[R|t])，所述Tx即所述固定数量的转换矩阵。

S4：对所述转换矩阵的参数进行松耦合求解得到稳定的相似变换；包括：

S41：计算所述转换矩阵Tx的旋转矩阵的中值与平移矩阵的中值；具体地，Tx中X个旋转矩阵R的中值为Rm，Tx中X个平移矩阵t的中值为tm；

S42：将所述转换矩阵Tx中的所有旋转矩阵R与所述旋转矩阵的中值Rm比较，将所述转换矩阵Tx中的所有平移矩阵t与所述平移矩阵的中值tm比较，剔除所述旋转矩阵R与所述旋转矩阵的中值Rm差的绝对值大于阈值或者平移矩阵t与平移矩阵的中值tm差的绝对值大于阈值的转换矩阵得到更新的转换矩阵。具体地，将Tx中abs(R-Rm)>Ω度或abs(t-tm)/tm>c的转换矩阵剔除，得到更新的转换矩阵，所述更新的转换矩阵为Y个6Dof转换矩阵Ty(Ty包含了y个[R|t])，通过以上剔除与中值差别大的点对的方法剔除定位误差大的位姿。具体地，所述Y个6Dof转换矩阵Ty的旋转矩阵R的中值为Rm2。所述Y个6Dof转换矩阵Ty所对应的视觉里程计位姿为P1中的Y个位姿P1y，所对应的视觉定位位姿为P2中的Y个位姿P2y。

S43：通过所述更新的转换矩阵对应的三维点对的位姿、所述相似变换的尺度参数s和所述旋转矩阵的中值Rm2计算所述相似变换的平移矩阵，得到稳定的相似变换。

具体地，X个P1y位姿的相机位置为p1(t10，t11，t12，......)，t10，t11等是三维坐标点，同时刻的X个P2y位姿的相机位置为p2(t20，t21，t22，......)，t20，t21等是三维坐标点；通过P1y中p1(包含Y个3D坐标点(x,y,z))与P2y中p2(包含Y个3D坐标点(x,y,z))计算p1到p2的转换关系，根据步骤S33所计算的所述相似变换的尺度参数s和步骤S42计算的Rm2计算出平移矩阵t12，具体地，t12＝p1-s*Rm2*t12。转换矩阵[Rm2|t12 s]即为所述视觉里程计坐标系与所述视觉定位坐标系之间的稳定的相似变换。由于Sim3估计出的转换矩阵经常存在角度误差，因此所述相似变换的旋转矩阵不采用所述Sim3的计算结果，而采用Rm2；而Sim3估计的平移矩阵不存在误差问题，因此可所述相似变换的平移矩阵可以采用Sim3估计的结果。采用上述方法，所述相似变换的旋转矩阵R、平移矩阵t、尺度参数s都是分开求解，不完全依靠一次Sim3对齐，因此该方法为松耦合求解。Sim3估计出的转换矩阵经常存在角度误差，分开求解利用内点位姿对的姿态计算更准确的旋转矩阵R，精度更高。

S5：将所述稳定的相似变换进行平滑转换得到平滑的相似变换，该步骤的目的为将高帧率的视觉里程计位姿平滑转换到视觉定位的坐标系。具体地，保留上一时刻的稳定的相似变换Trans1([R1|t1 s1])与当前时刻的稳定的相似变换Trans2([R2|t2 s2])。具体地，可以每次利用N个内点求解出一个相似变换矩阵，例如每间隔一秒，利用滑动窗口计算一次，前一时刻计算得到Trans1([R1|t1 s1])，1s后用新的滑动窗口再计算一次得到Trans2([R2|t2 s2])。两次稳定转换矩阵可能存在持续的变化，为了实现视觉上位姿平滑效果，通过加权实现稳定的转换矩阵在n秒内的平滑变动，分别对s,R和t插值，timestamp1是求解得到Trans1时的时间，timestamp2是求解得到Trans2时的时间。具体步骤如下：

S51：根据时间戳计算插值比例ratio，ratio＝min(n,(timestamp1-timestamp2))/n；

S52：根据上一次Sim3变换计算的尺度参数s1和本次Sim3变换计算的尺度参数s2按照所述插值比例ratio计算插值后的尺度参数s，s＝s1*(1-ratio)+s2*ratio；

S53：根据上一次Sim3变换计算的位移矩阵t1和本次Sim3变换计算的位移矩阵t2按照插值比例ratio计算插值后的位移矩阵t，t＝t1*(1-ratio)+t2*ratio；

S54：根据上一次Sim3变换计算的旋转矩阵R1和本次sim3变换计算角度转换矩阵R2按照插值比例ratio计算插值后的旋转矩阵R，R＝R1*(1-ratio)+R2*ratio；

S55：视觉里程计位姿P1通过平滑的相似变换T([R|t s])对齐到视觉定位位姿P2坐标系得到位姿P3。

S6：输出与视觉里程计同帧率的视觉定位位姿P3。

实施例二：

本实施例提供一种视觉定位与视觉里程计位姿融合的系统，请参阅图2，所述系统包括视觉定位模块11、视觉里程计模块12、融合模块13、滑动窗口模块14、位姿对齐模块15、求解模块16、坐标系转换模块17、输出位姿模块18，其中：

所述视觉定位模块11用于获取当前时刻图像的视觉定位位姿。具体地，设备A的图像定位，获取了图像在视觉定位中time1时刻的位姿(位置和姿态)P2。包括但不限于图片跟踪定位、3D模型跟踪定位、三维空间定位(VPS)、自动驾驶导航定位、手机AR扫二维码定位等一系列通过相机计算当前设备位姿的方法。

在本实施例中，所述视觉里程计模块12用于获取当前时刻图像的视觉里程计位姿。具体地，设备A的里程计中获取到time1时刻的位姿(位置和姿态)P1。这里的里程计包括但不限于里程计(VIO、VISLAM)、纯视觉里程计(VO、SLAM)、纯惯导(IMU，GPS,RTK)等计算里程计的方法。

在本实施例中，所述融合模块13用于将所述视觉里程计位姿与所述视觉定位位姿组成融合帧。具体地，所述视觉定位模块11、视觉里程计模块12分别获取当前图像的视觉定位的6Dof的位姿P1、视觉里程计的6Dof的位姿P2，所述融合模块13获取当前时间戳time1，所述融合帧fusionFrame包括时间戳time1、图像的里程计位姿P1(6Dof位姿[R|t])和视觉定位位姿P2(6Dof位姿[R|t])。

在本实施例中，所述滑动窗口模块14用于保存最近时间内固定数量的所述融合帧fusionFrame，所述固定数量大于三个；具体地，所述滑动窗口模块14持续存储当前时刻融合帧fusionFrame，当所述滑动窗口模块14的融合帧数量大于所述固定数量时，删除时间戳最早的融合帧。

在本实施例中，所述位姿对齐模块15用于通过所述固定数量的融合帧求解所述视觉里程计位姿与所述视觉定位位姿的相似变换的尺度参数和固定数量的转换矩阵；具体地，所述位姿对齐模块15读取所述滑动窗口模块14的缓存模块中的N个融合帧fusionFrame，每个融合帧fusionFrame中的里程计位姿P1与视觉定位位姿P2组成一个位姿对。N个融合帧fusionFrame提供了N个位姿对。具体地，所述位姿对齐模块15通过RANSAC的Sim3算法将所述融合帧fusionFrame中的视觉里程计位姿P1对齐至视觉定位位姿P2的坐标系，所述Sim3算法为使用至少三对匹配点来进行相似变换(similarity transformation)的求解，进而解出两个坐标系之间的旋转矩阵R、平移矩阵t和尺度参数s。具体地，N个融合帧fusionFrame包括N个里程计位姿P1，对应的位姿P1包括N个旋转矩阵和平移矩阵([R10|t10]，[R11|t11]，[R12|t12]，......)，其中R1n为第n个里程计位姿的3*3旋转矩阵，t1n为第n个里程计位姿的3*1平移矩阵。N个融合帧fusionFrame还包括N个视觉定位位姿，对应的位姿P2包括N个旋转矩阵和平移矩阵([R20|t20]，[R21|t21]，[R22|t22]，......)，其中R2n为第n个定位位姿的3*3旋转矩阵，t2n为第n个定位位姿的3*1平移矩阵。所述视觉里程计位姿P1的相机位置为p1(t10，t11，t12，......)，所述视觉定位位姿P2的相机位置为p2(t20，t21，t22，......)，N个数据组成了N个三维点对。具体地，求解Sim3变换的过程，可以理解为求解两个坐标系的相似变换。其中，p2＝sR(p1)+t，通过RANSAC剔除外点后，得到X个融合帧fusionFrame作为内点，只需要超过三张图存在三维点对即可求解出坐标系的转换矩阵的尺度参数s，3*3旋转矩阵R，3*1平移矩阵t。X个融合帧fusionFrame计算出X个6Dof转换矩阵Tx(T包含了X个[R|t])，每一个转换矩阵[R|t]表示视觉里程计位姿与视觉定位位姿的转换关系。

在本实施例中，所述求解模块16用于对当前时刻的所述转换矩阵的参数进行松耦合求解得到稳定的相似变换；具体地，所述求解模块剔除所述转换矩阵中所述旋转矩阵与所述旋转矩阵的中值差的绝对值大于阈值或者平移矩阵与平移矩阵的中值差的绝对值大于阈值的转换矩阵得到更新的转换矩阵，通过所述更新的转换矩阵对应的三维点对的位姿、所述相似矩阵的尺度参数和所述旋转矩阵的中值计算所述相似变换的平移矩阵，得到稳定的相似变换。具体地，Tx中X个旋转矩阵R的中值是Rm，Tx中X个位移矩阵t的中值是tm，将Tx中abs(R-Rm)>Ω度或abs(t-tm)/tm>c的转换矩阵剔除，得到Y个6Dof转换矩阵Ty(Ty包含了y个[R|t])，所述Y个Dof转换矩阵Ty的旋转矩阵R的中值为Rm2。y个转换矩阵所对应的位姿是P1中的Y个位姿P1y，P2中的Y个位姿P2y。P1y中p1(包含Y个3D坐标点(x,y,z))与P2y中p2(包含Y个3D坐标点(x,y,z))，计算p1到p2的转换关系，根据所述相似变换的尺度参数s和Rm2计算平移矩阵t12，由此得到稳定的相似变换[Rm2|t12 s]，即所述视觉里程计坐标系与所述视觉定位坐标之间的转换关系。

在本实施例中，所述坐标系转换模块17用于将所述稳定的相似变换进行平滑转换得到平滑的相似变换，根据所述平滑的相似变换将所述视觉里程计位姿转换至所述视觉定位位姿坐标系；具体地，所述坐标系转换模块17通过插值法分别对上一时刻的稳定的相似变换与当前时刻的稳定的相似变换的尺度参数、旋转矩阵和平移矩阵进行计算，得到平滑的相似变换。具体地，所述坐标系转换模块17保留了两个相似变换，一个是上一次所述求解模块16输出的稳定的相似变换Trans1([R1|t1 s1])，以及最新的所述求解模块16输出的稳定的相似变换Trans2([R2|t2 s2])。两次稳定的相似变换可能存在持续的变化，为了实现视觉上位姿平滑效果，通过加权实现所述两次稳定的相似变换在n秒内的平滑变动。分别对s,R和t插值，timestamp1是求解得到Trans1时的时间，timestamp2是求解得到Trans2时的时间，所述坐标系转换模块17根据时间戳计算插值比例ratio，ratio＝min(n,(timestamp1-timestamp2))/n；根据上一次Sim3变换计算的尺度参数s1和本次sim3变换计算的尺度参数s2按照所述插值比例ratio计算插值后的尺度参数s，s＝s1*(1-ratio)+s2*ratio；根据上一次Sim3变换计算的位移矩阵t1和本次sim3变换计算的位移矩阵t2按照插值比例ratio计算插值后的位移矩阵t，t＝t1*(1-ratio)+t2*ratio；根据上一次Sim3变换计算的旋转矩阵R1和本次Sim3变换计算旋转矩阵R2按照插值比例ratio计算插值后的旋转矩阵R，R＝R1*(1-ratio)+R2*ratio；所述坐标系转换模块17将所述视觉里程计位姿P1通过所述平滑的相似变换T([R|t s])对齐到视觉定位位姿P2坐标系得到位姿P3。

在本实施例中，所述输出位姿模块18用于输出与视觉里程计同帧率的视觉定位位姿P3。

实施例三：

本实施例提供另一种视觉定位与视觉里程计位姿融合的系统，请参阅图3，所述系统包括存储器21和处理器22，其中：

所述存储器21，用于存储代码和相关数据；

所述处理器22，用于调用所述存储器21中的数据，执行所述存储器21中的代码能实施前述视觉定位与视觉里程计位姿融合的方法和步骤，具体方法和步骤参照前述实施例的描述，在此不再赘述。

在本实施例中，所述存储器21可能包含易失性存储器，例如，随机存取存储器(random access memory，RAM)，所述RAM可以包括静态RAM或动态RAM。所述存储器21也可能包括非易失性存储器(non-volatile memory)，例如只读存储器(read-only memory，PROM)、可编程只读存储器(programmable read-only memory，PROM)、可擦写可编程只读存储器(erasable programmable read-only memory，EPROM)、电可擦写可编程只读存储器(electrically erasable programmable read-only memory，EEPROM)或闪存(flashmemory)。所述存储器21还可能是外部闪存、至少一个磁盘存储器或缓存器。

在本实施例中，所述处理器22可以是中央处理单元(Central Processing Unit,CPU)，所述处理器22还可以是其他通用控制处理器、数字信号处理器(Digital SignalProcessing，DSP)、专用集成电路(Application Specific Integrated Circuit，ASIC)、现场可编程门阵列(Field-Programmable Gate Array，FPGA)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件等。所述通用控制处理器可以是微控制处理器或者是任何常规的控制处理器，例如单片机等。

显然，本发明的上述实施例仅仅是为清楚地说明本发明所做的举例，而并非是对本发明的实施方式的限定，对于所属领域的普通技术人员而言，在上述说明的基础上还可以做出其他不同形式的变化或变动，本发明所例举的实施例无法对所有的实施方式予以穷尽，凡是属于本发明的技术方案所引申出的显而易见的变化或变动仍处于本发明的保护范围之列。在本发明中提及的所有文献都在本申请中引用作为参考，就如同一篇文献被单独引用为参考那样。

Claims

1.一种视觉定位与视觉里程计位姿融合的方法，其特征在于，所述方法包括：

步骤1：获取当前时刻图像的视觉里程计位姿和视觉定位位姿，所述视觉里程计位姿与所述视觉定位位姿组成融合帧；

步骤6：输出与视觉里程计同帧率的视觉定位位姿。

2.根据权利要求1所述的视觉定位与视觉里程计位姿融合的方法，其特征在于，所述视觉里程计位姿与所述视觉定位位姿均包括6Dof的位姿，所述融合帧包括时间戳、图像的视觉里程计位姿与视觉定位位姿。

3.根据权利要求2所述的视觉定位与视觉里程计位姿融合的方法，其特征在于，保存最近时间内固定数量的所述融合帧，包括：

持续将当前时刻融合帧存入滑动窗口；

4.根据权利要求1所述的视觉定位与视觉里程计位姿融合的方法，其特征在于，通过所述固定数量的融合帧求解所述视觉里程计位姿与所述视觉定位位姿的相似变换的尺度参数和固定数量的转换矩阵，包括：

读取当前时刻所述滑动窗口内的固定数量的融合帧；

5.根据权利要求4所述的视觉定位与视觉里程计位姿融合的方法，其特征在于，对所述转换矩阵的参数进行松耦合求解得到稳定的相似变换，包括：

6.根据权利要求5所述的视觉定位与视觉里程计位姿融合的方法，其特征在于，将所述稳定的相似变换进行平滑转换得到平滑的相似变换，包括：

7.一种视觉定位与视觉里程计位姿融合的系统，其特征在于，所述系统包括视觉定位模块、视觉里程计模块、融合模块、滑动窗口模块、位姿对齐模块、求解模块、坐标系转换模块、输出位姿模块，其中：

所述视觉定位模块用于获取当前时刻图像的视觉定位位姿；

8.根据权利要求7所述的视觉定位与视觉里程计位姿融合的系统，其特征在于，所述视觉定位模块、视觉里程计模块分别获取当前图像的视觉定位的6Dof的位姿、视觉里程计的6Dof的位姿，所述融合模块获取当前时间戳，所述融合帧包括所述时间戳、图像的视觉里程计位姿与视觉定位位姿。

9.根据权利要求8所述的视觉定位与视觉里程计位姿融合的系统，其特征在于，所述滑动窗口模块持续存储当前时刻融合帧，当所述滑动窗口模块的融合帧数量大于所述固定数量时，删除时间戳最早的融合帧。

10.根据权利要求9所述的视觉定位与视觉里程计位姿融合的系统，其特征在于，所述位姿对齐模块通过RANSAC的Sim3算法将所述滑动窗口模块中融合帧中的视觉里程计位姿对齐至视觉定位位姿的坐标系，剔除所述视觉定位位姿中的外点后，得到所述视觉定位位姿中的内点，所述视觉定位位姿中的内点与对应的视觉里程计位姿中的三维点组成内点位姿对，通过所述内点位姿对计算当前时刻所述视觉里程计位姿至所述视觉定位位姿的相似变换的尺度参数和固定数量的转换矩阵，所述当前时刻的相似变换的参数包括尺度参数、旋转矩阵和平移矩阵，所述固定数量的转换矩阵的参数包括旋转矩阵和平移矩阵。

11.根据权利要求10所述的视觉定位与视觉里程计位姿融合的系统，其特征在于，所述求解模块剔除所述转换矩阵中所述旋转矩阵与所述旋转矩阵的中值差的绝对值大于阈值或者平移矩阵与平移矩阵的中值差的绝对值大于阈值的转换矩阵得到更新的转换矩阵，通过更新的转换矩阵对应的三维点对的位姿、所述相似变换的尺度参数和所述旋转矩阵的中值计算所述相似变换的平移矩阵，得到稳定的相似变换。

12.根据权利要求11所述的视觉定位与视觉里程计位姿融合的系统，其特征在于，所述坐标系转换模块通过插值法分别对上一时刻的稳定的相似变换与当前时刻的稳定的相似变换的尺度参数、旋转矩阵和平移矩阵进行计算，得到平滑的相似变换。

13.一种视觉定位与视觉里程计位姿融合的系统，其特征在于，所述系统包括存储器和处理器，其中：

所述存储器，用于存储代码和相关数据；

所述处理器，用于调用所述存储器中的数据，执行所述存储器中的代码能实施如权利要求1至6任一项所述的方法和步骤。