CN111220155A - 基于双目视觉惯性里程计估计位姿的方法、装置与处理器 - Google Patents

Abstract

本申请提供了一种基于双目视觉惯性里程计估计位姿的方法、装置、存储介质与处理器。该方法包括：获取第一姿态角、第二姿态角和位移，第一姿态角为通过惯性测量单元获取的载体的姿态角，从双目视觉里程计获取的对应于载体的多帧图像中获取第二姿态角和位移；获取第一姿态角和第二姿态角的加权平均值，加权平均值为载体的融合姿态角；根据融合姿态角更新位移，通过将惯性测量单元和双目视觉里程计获取的姿态角进行融合，提升了融合系统的鲁棒性，降低了融合算法运算时间，使得融合算法调试更灵活，视觉里程计的姿态角的精度得到明显提升，两者通过融合提高了单纯的视觉定位算法的定位精度。

Description

基于双目视觉惯性里程计估计位姿的方法、装置与处理器

技术领域

本申请涉及机器人领域，具体而言，涉及一种基于双目视觉惯性里程计估计位姿的方法、装置、存储介质与处理器。

背景技术

目前，随着小型化，低功耗，经济性移动机器人及无人机的大力发展，需要迫切解决在低功耗平台下进行定位功能的实现，视觉定位方法作为自主定位的一种方式，能有效解决轮式里程计在恶劣环境(比如建筑工地)打滑引起的误差，同时在建筑机器人开发中，存在一些没有轮子或者运动模型比较难以建模，此时只能依靠视觉里程计来实现粗定位功能。

传统以非线性优化为主的视觉定位算法由于对运算资源要求过大，且存在位姿输出延时，需要进行改进方能满足小型建筑机器人低功耗，嵌入式处理器，实时定位功能要求。

也有部分的算法直接去掉视觉里程计中较耗时模块后端优化算法，降低对运算资源的要求，但导致了视觉里程计的定位精度下降，鲁棒性降低。

在背景技术部分中公开的以上信息只是用来加强对本文所描述技术的背景技术的理解，因此，背景技术中可能包含某些信息，这些信息对于本领域技术人员来说并未形成在本国已知的现有技术。

发明内容

本申请的主要目的在于提供一种基于双目视觉惯性里程计估计位姿的方法、装置、存储介质与处理器，以解决现有技术中视觉定位算法的定位精度较低的问题。

为了实现上述目的，根据本申请的一个方面，提供了一种基于双目视觉惯性里程计估计位姿的方法，包括：获取第一姿态角、第二姿态角和位移，所述第一姿态角为通过惯性测量单元获取的载体的姿态角，从双目视觉里程计获取的对应于所述载体的多帧图像中获取所述第二姿态角和所述位移；获取所述第一姿态角和所述第二姿态角的加权平均值，所述加权平均值为所述载体的融合姿态角；根据所述融合姿态角更新所述位移。

进一步地，所述双目视觉里程计包括双目相机，在获取第一姿态角、第二姿态角和位移之前，所述方法还包括：初始化所述双目相机的参数和所述惯性测量单元的参数；采集所述双目相机的左图像和右图像，采集所述惯性测量单元的原始输出；初始化所述双目相机采集的图像的特征点。

进一步地，初始化所述双目相机采集的图像的特征点，包括：获取所述双目相机采集的第一帧图像的特征点和特征点深度值。

进一步地，获取所述双目相机采集的第一帧图像的特征点和特征点深度值，包括：提取所述双目相机采集到的第一帧图像的所述左图像的第一特征点并保存所述第一特征点；在所述第一帧图像的所述右图像上跟踪所述左图像的所述第一特征点，获取第二特征点，并保存所述第二特征点，所述第一特征点和所述第二特征点构成特征点对；对所述特征点对进行畸变矫正；采用矫正后的所述特征点对，计算所述特征点对的所述特征点深度值。

进一步地，在初始化所述双目相机采集的图像的特征点之后，获取第一姿态角包括：获取第N帧图像和第N+1帧图像，N为大于1的整数；获取第一时刻和第二时刻，所述第一时刻为获取所述第N帧图像的时刻，所述第二时刻为获取所述第N+1帧图像的所述时刻；获取处于第三时刻的所述第一姿态角，所述第三时刻在所述第一时刻和所述第二时刻之间。

进一步地，在初始化所述双目相机采集的图像的特征点之后，获取第二姿态角和位移包括：获取第三特征点，所述第三特征点为所述第N帧图像的左图像上的特征点；在所述第N+1帧图像的左图像上跟踪所述第三特征点，获取第四特征点，所述第四特征点为所述第N+1帧图像的左图像上的特征点；在所述第三特征点的数量大于第一预定值的情况下，对所述第三特征点和所述第四特征点进行畸变矫正；采用矫正后的所述第三特征点和所述第四特征点，获取所述第二姿态角和所述位移。

进一步地，位姿包括所述第二姿态角和所述位移，获取所述第二姿态角和所述位移，还包括：比较所述第N+1帧图像的所述位姿与所述第N帧图像的所述位姿，得到比较结果，N为大于1的整数；根据所述比较结果检测是否有异常值；在有所述异常值的情况下，使用所述第N帧图像的所述位姿替换所述第N+1帧图像的所述位姿，且删除所述第N+1帧图像的所述位姿，并重新跟踪第N+M帧图像，M为大于等于2的整数；在没有所述异常值的情况下，根据第N+M帧图像的所述位姿获取所述第二姿态角和所述位移。

进一步地，获取第一姿态角，还包括：设置时间阈值；在采集所述第一姿态角的时间达到所述时间阈值的情况下，采用上一次获取的所述第一姿态角作为惯性测量单元姿态更新的初值；根据所述初值更新所述第一姿态角。

进一步地，根据所述融合姿态角更新所述位移，包括：根据所述融合姿态角，在固定姿态矩阵的情况下，利用预定数量的2D-3D点对更新所述位移。

进一步地，获取所述双目相机采集的第一帧图像的特征点和特征点深度值，还包括：获取第N帧图像正在跟踪的所述特征点的数量，N为大于1的整数；在所述特征点的数量小于第二预定值的情况下，增加所述特征点和对应的所述特征点深度值。

根据本申请的另一个方面，提供了一种基于双目视觉惯性里程计估计位姿的装置，包括：第一获取单元，获取第一姿态角、第二姿态角和位移，所述第一姿态角为通过惯性测量单元获取的载体的姿态角，从双目视觉里程计获取的对应于所述载体的多帧图像中获取所述第二姿态角和所述位移；第二获取单元，获取所述第一姿态角和所述第二姿态角的加权平均值，所述加权平均值为所述载体的融合姿态角；更新单元，用于根据所述融合姿态角更新所述位移。

根据本申请的又一个方面，提供了一种存储介质，所述存储介质包括存储的程序，其中，所述程序执行任意一种所述的方法。

根据本申请的再一个方面，提供了一种处理器，所述处理器用于运行程序，其中，所述程序运行时执行任意一种所述的方法。

应用本申请的技术方案，通过惯性测量单元获取的载体的第一姿态角，通过双目视觉里程计获取载体的第二姿态角和位移，进而获取第一姿态角和第二姿态角的加权平均值，再根据融合姿态角新载体的位移，单纯的视觉里程计容易受到外界干扰(比如动态特征点)，将导致姿态角出现较大误差，通过将惯性测量单元和双目视觉里程计获取的姿态角进行融合，提升了融合系统的鲁棒性，降低了融合算法运算时间，使得融合算法调试更灵活，视觉里程计的姿态角的精度得到明显提升，尤其在旋转运动中，单纯视觉里程计容易出现比较大的旋转测量误差，而惯性测量单元表现稳定，两者通过融合提高了单纯的视觉定位算法的定位精度，单纯视觉里程计和惯性测量单元均存在累积误差，两者通过本发明算法融合，累积误差有所降低，从而提高了视觉定位算法的定位精度。

附图说明

构成本申请的一部分的说明书附图用来提供对本申请的进一步理解，本申请的示意性实施例及其说明用于解释本申请，并不构成对本申请的不当限定。在附图中：

图1示出了根据本申请实施例的一种基于双目视觉惯性里程计估计位姿的方法的流程图；

图2示出了根据本申请实施例的一种基于双目视觉惯性里程计估计位姿的装置的示意图；

图3示出了根据本申请实施例的一种具体的基于双目视觉惯性里程计估计位姿的方法流程图；以及

图4示出了根据本申请实施例的一种具体的基于双目视觉惯性里程计估计位姿的方法的算法原理图。

具体实施方式

需要说明的是，在不冲突的情况下，本申请中的实施例及实施例中的特征可以相互组合。下面将参考附图并结合实施例来详细说明本申请。

为了使本技术领域的人员更好地理解本申请方案，下面将结合本申请实施例中的附图，对本申请实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本申请一部分的实施例，而不是全部的实施例。基于本申请中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都应当属于本申请保护的范围。

需要说明的是，本申请的说明书和权利要求书及上述附图中的术语“第一”、“第二”等是用于区别类似的对象，而不必用于描述特定的顺序或先后次序。应该理解这样使用的数据在适当情况下可以互换，以便这里描述的本申请的实施例。此外，术语“包括”和“具有”以及他们的任何变形，意图在于覆盖不排他的包含，例如，包含了一系列步骤或单元的过程、方法、系统、产品或设备不必限于清楚地列出的那些步骤或单元，而是可包括没有清楚地列出的或对于这些过程、方法、产品或设备固有的其它步骤或单元。

应该理解的是，当元件(诸如层、膜、区域、或衬底)描述为在另一元件“上”时，该元件可直接在该另一元件上，或者也可存在中间元件。而且，在说明书以及权利要求书中，当描述有元件“连接”至另一元件时，该元件可“直接连接”至该另一元件，或者通过第三元件“连接”至该另一元件。

为了便于描述，以下对本申请实施例涉及的部分名词或术语进行说明：

视觉里程计：在机器人领域中，视觉里程计通过分析处理相关图像序列来确定机器人的位置和姿态。

惯性测量单元，IMU(Inertial measurement unit)是用来测量物体三轴姿态角以及加速度的装置，一个IMU包含了三个单轴的加速度计和三个单轴的陀螺，加速度计检测物体在载体坐标系统独立三轴的加速度信号，而陀螺检测载体相对于导航坐标系的角速度信号，测量物体在三维空间中的角速度和加速度，并以此解算出物体的姿态。

LK光流法：即Lucas-Kanade光流法，通过在第一帧图像中找到特征点，后续图像跟踪第一帧图像中的特征点以实现特征点的遍历。

正如背景技术中所介绍的，现有技术中的视觉定位算法的定位精度较低，为解决如上视觉定位算法的定位精度较低的问题，本申请的实施例提供了一种基于双目视觉惯性里程计估计位姿的方法、装置、存储介质与处理器。

根据本申请的实施例，提供了一种基于双目视觉惯性里程计估计位姿的方法。

图1是根据本申请实施例的基于双目视觉惯性里程计估计位姿的方法的流程图。如图1所示，该方法包括以下步骤：

步骤S101，获取第一姿态角、第二姿态角和位移，上述第一姿态角为通过惯性测量单元获取的载体的姿态角，从双目视觉里程计获取的对应于上述载体的多帧图像中获取上述第二姿态角和上述位移；

步骤S102，获取上述第一姿态角和上述第二姿态角的加权平均值，上述加权平均值为上述载体的融合姿态角；

步骤S103，根据融合姿态角更新位移。

上述方案中，通过惯性测量单元获取的载体的第一姿态角，通过双目视觉里程计获取载体的第二姿态角和位移，进而获取第一姿态角和第二姿态角的加权平均值，再根据融合姿态角新载体的位移，单纯的视觉里程计容易受到外界干扰(比如动态特征点)，将导致姿态角出现较大误差，通过将惯性测量单元和双目视觉里程计获取的姿态角进行融合，提升了融合系统的鲁棒性，降低了融合算法运算时间，使得融合算法调试更灵活，视觉里程计的姿态角的精度得到明显提升，尤其在旋转运动中，单纯视觉里程计容易出现比较大的旋转测量误差，而惯性测量单元表现稳定，两者通过融合提高了单纯的视觉定位算法的定位精度，单纯视觉里程计和惯性测量单元均存在累积误差，两者通过本发明算法融合，累积误差有所降低，从而提高了视觉定位算法的定位精度。

具体地，通过设定合适的第二姿态角的比重和第一姿态角的比重，加权融合两者的姿态角，获取融合姿态角：attitude＝weightVo×attitudeVo+weightImu×attitudeImu，其中，attitude表示融合姿态角，attitudeVo表示第二姿态角，weightVo表示第二姿态角的比重，attitudeImu表示第一姿态角，weightImu表示第一姿态角的比重。

需要说明的是，第一姿态角的比重和第二姿态角的比重根据实际情况进行设置，例如，第一姿态角所占的比重为70％，第二姿态角所占的比重为30％，当然，本领域技术人员可以根据实际情况选择合适的第一姿态角的比重和第二姿态角的比重。

需要说明的是，除了通过惯性测量单元获取第一姿态角，还可以通过3D激光雷达等获取第一姿态角，优选地，通过惯性测量单元获取第一姿态角。

需要说明的是，在附图的流程图示出的步骤可以在诸如一组计算机可执行指令的计算机系统中执行，并且，虽然在流程图中示出了逻辑顺序，但是在某些情况下，可以以不同于此处的顺序执行所示出或描述的步骤。

本申请的一种实施例，上述双目视觉里程计包括双目相机，在获取第一姿态角、第二姿态角和位移之前，上述方法还包括：初始化上述双目相机的参数和上述惯性测量单元的参数；采集上述双目相机的左图像和右图像，采集上述惯性测量单元的原始输出；初始化上述双目相机采集的图像的特征点，通过初始化双目相机的参数、惯性测量单元的参数以及双目相机采集的图像的特征点，提高了视觉定位算法的定位精度，具体包括双目相机小孔成像模型参数，惯性测量单元器件误差模型参数等其它相关系数初始化。

本申请的另一种实施例，初始化上述双目相机采集的图像的特征点，包括获取上述双目相机采集的第一帧图像的特征点和特征点深度值，利用LK光流法，将第一帧图像用作特征点初始化，保证了双目视觉惯性里程计估计位姿的准确性。

本申请的又一种实施例，获取上述双目相机采集的第一帧图像的特征点和特征点深度值，包括：提取上述双目相机采集到的第一帧图像的上述左图像的第一特征点并保存上述第一特征点；在上述第一帧图像的上述右图像上跟踪上述左图像的上述第一特征点，获取第二特征点，并保存上述第二特征点，上述第一特征点和上述第二特征点构成特征点对；对上述特征点对进行畸变矫正；采用矫正后的上述特征点对，计算上述特征点对的上述特征点深度值，具体地，第一帧图像用作特征点初始化，具体通过对左图提取Shi-Tomas角点，保存特征点；利用LK光流法在右图上跟踪上一步骤在左图上提取的特征点，保存跟踪点对；根据径向畸变模型和切向畸变模型对特征点对进行畸变校正；通过左右图像对应的特征点对三角化计算特征点深度值，特征点深度值的计算公式为：z＝fb/d，其中z为特征点深度值，f为相机焦距，b为双目基线长度，d为两对应特征点的像素点差(视差)，通过获取第一帧图像的特征点和特征点深度值，保证了双目视觉惯性里程计估计位姿的准确性。

本申请的另一种实施例，在初始化上述双目相机采集的图像的特征点之后，获取第一姿态角包括：获取第N帧图像和第N+1帧图像，N为大于1的整数；获取第一时刻和第二时刻，上述第一时刻为获取上述第N帧图像的时刻，上述第二时刻为获取上述第N+1帧图像的上述时刻；获取处于第三时刻的上述第一姿态角，上述第三时刻在上述第一时刻和上述第二时刻之间，通过获取相邻的两帧图像之间的第一姿态角，即根据前后两帧图像的时间戳，提取在两帧图像间的惯性测量单元的测量值，保存；再根据惯性测量单元给出的测量值，通过四阶拢格库塔法更新姿态矩阵，进而得到准确的第一姿态角，进而保证了双目视觉惯性里程计估计位姿的准确性。

本申请的再一种实施例，在初始化上述双目相机采集的图像的特征点之后，获取第二姿态角和位移包括：获取第三特征点，上述第三特征点为上述第N帧图像的左图像上的特征点；在上述第N+1帧图像的左图像上跟踪上述第三特征点，获取第四特征点，上述第四特征点为上述第N+1帧图像的左图像上的特征点；在上述第三特征点的数量大于第一预定值的情况下，对上述第三特征点和上述第四特征点进行畸变矫正；采用矫正后的上述第三特征点和上述第四特征点，获取上述第二姿态角和上述位移，通过第三特征点和第四特征点，得到了准确的第二姿态角和位移，进而保证了双目视觉惯性里程计估计位姿的准确性，上述第一预定值可以为10、20或30等，且通过对图像特征点的重复跟踪使用，降低了图像特征点重新提取时间，降低了视觉里程计定位误差累积。

本申请的一种实施例，位姿包括上述第二姿态角和上述位移，获取上述第二姿态角和上述位移，还包括：比较上述第N+1帧图像的上述位姿与上述第N帧图像的上述位姿，得到比较结果，N为大于1的整数；根据上述比较结果检测是否有异常值；在有上述异常值的情况下，使用上述第N帧图像的上述位姿替换上述第N+1帧图像的上述位姿，且删除上述第N+1帧图像的上述位姿，并重新跟踪第N+M帧图像，M为大于等于2的整数；在没有上述异常值的情况下，根据第N+M帧图像的上述位姿获取上述第二姿态角和上述位移，利用2D-3D点对进行位姿的求解，其中使用RANSC(随机采样一致性算法)剔除掉误匹配特征点；计算当前帧图像的位姿(R，T)与上一帧图像的位姿(R，T)进行对比，检测是否为异常值，假如为异常值则用上一帧图像位姿替换为当前帧图像的位姿，删除当前帧图像的相关特征点，并返回重新跟踪下一帧图像，进一步地保证了第二姿态角和位移的准确性，进而保证了双目视觉惯性里程计估计位姿的准确性，具体地，异常值表示上述第N+1帧图像的上述位姿与上述第N帧图像的上述位姿的差值较大，由于载体一般是连续运动的，差值较大证明第N+1帧图像的上述位姿有较大的偏差，例如，比如用户在开车的过程中，GPS在10：00时候定位离终点还有1000公里(相当于第N帧图像)，那么GPS在10：10时候(相当于第N+1帧图像)定位离终点还差100公里，这1000-100＝900公里，这就“异常”，因为10分钟行驶900公里是违背常规的。

本申请的一种实施例，获取第一姿态角，还包括：设置时间阈值；在采集上述第一姿态角的时间达到上述时间阈值的情况下，采用上一次获取的上述第一姿态角作为惯性测量单元姿态更新的初值；根据上述初值更新上述第一姿态角，为了降低惯性器件的累积误差，根据惯性器件性能，设定时间阈值，当达到时间阈值则利用上一步获取的姿态设为惯性测量单元姿态更新的初值，进而保证了获取的第一姿态角的准确性，进而保证了双目视觉惯性里程计估计位姿的准确性。

本申请的一种实施例，根据上述融合姿态角更新上述位移，包括：根据上述融合姿态角，在固定姿态矩阵的情况下，利用预定数量的2D-3D点对更新上述位移，单纯视觉里程计计算的姿态和位移，是由点对最小投影误差优化得到，因此当姿态出现误差，势必导致位移测量存在误差，通过本发明固定融合后的姿态角，再去进行位移值的求解算法，很大程度降低了载体位移轨迹偏差，利用融合优化后姿态角，挑选前10个2D-3D点对在固定姿态矩阵的情况下，求解位移值，达到优化视觉里程计的位置效果，进而保证了双目视觉惯性里程计估计位姿的准确性，当然，实际应用中可以选择20个2D-3D点对、30个2D-3D点对以及40个2D-3D点对等。

本申请的一种实施例，获取上述双目相机采集的第一帧图像的特征点和特征点深度值，还包括：获取第N帧图像正在跟踪的上述特征点的数量，N为大于1的整数；在上述特征点的数量小于第二预定值的情况下，增加上述特征点和对应的上述特征点深度值，即需要征点的数量达到一定数值的情况下，才能保证初始化双目相机采集的图像的特征点的准确性，进而保证双目视觉惯性里程计估计位姿的准确性，上述第二预定值可以为10、20或30等。

本申请实施例还提供了一种基于双目视觉惯性里程计估计位姿的装置，需要说明的是，本申请实施例的基于双目视觉惯性里程计估计位姿的装置可以用于执行本申请实施例所提供的用于基于双目视觉惯性里程计估计位姿的方法。以下对本申请实施例提供的基于双目视觉惯性里程计估计位姿的装置进行介绍。

图2是根据本申请实施例的基于双目视觉惯性里程计估计位姿的装置的示意图。如图2所示，该装置包括：

第一获取单元10，获取第一姿态角、第二姿态角和位移，上述第一姿态角为通过惯性测量单元获取的载体的姿态角，从双目视觉里程计获取的对应于上述载体的多帧图像中获取上述第二姿态角和上述位移；

第二获取单元20，获取上述第一姿态角和上述第二姿态角的加权平均值，上述加权平均值为上述载体的融合姿态角；

更新单元30，用于根据上述融合姿态角更新上述位移。

上述方案中，第一获取单元通过惯性测量单元获取的载体的第一姿态角，通过双目视觉里程计获取载体的第二姿态角和位移，第二获取单元获取第一姿态角和第二姿态角的加权平均值，更新单元根据融合姿态角新载体的位移，单纯的视觉里程计容易受到外界干扰(比如动态特征点)，将导致姿态角出现较大误差，通过将惯性测量单元和双目视觉里程计获取的姿态角进行融合，提升了融合系统的鲁棒性，降低了融合算法运算时间，使得融合算法调试更灵活，视觉里程计的姿态角的精度得到明显提升，尤其在旋转运动中，单纯视觉里程计容易出现比较大的旋转测量误差，而惯性测量单元表现稳定，两者通过融合提高了单纯的视觉定位算法的定位精度，单纯视觉里程计和惯性测量单元均存在累积误差，两者通过本发明算法融合，累积误差有所降低，从而提高了视觉定位算法的定位精度。

具体地，通过设定合适的第二姿态角的比重和第一姿态角的比重，加权融合两者的姿态角，获取融合姿态角：attitude＝weightVo×attitudeVo+weightImu×attitudeImu，其中，attitude表示融合姿态角，attitudeVo表示第二姿态角，weightVo表示第二姿态角的比重，attitudeImu表示第一姿态角，weightImu表示第一姿态角的比重。

需要说明的是，第一姿态角的比重和第二姿态角的比重根据实际情况进行设置，例如，第一姿态角所占的比重为70％，第二姿态角所占的比重为30％，当然，本领域技术人员可以根据实际情况选择合适的第一姿态角的比重和第二姿态角的比重。

需要说明的是，除了通过惯性测量单元获取第一姿态角，还可以通过3D激光雷达等获取第一姿态角，优选地，通过惯性测量单元获取第一姿态角。

本申请的一种实施例，上述装置还包括第一初始化单元、采集单元和第二初始化单元，第一初始化单元用于上述双目视觉里程计包括双目相机，在获取第一姿态角、第二姿态角和位移之前，初始化上述双目相机的参数和上述惯性测量单元的参数；采集单元用于上述双目视觉里程计包括双目相机，在获取第一姿态角、第二姿态角和位移之前，采集上述双目相机的左图像和右图像，采集上述惯性测量单元的原始输出；第二初始化单元用于上述双目视觉里程计包括双目相机，在获取第一姿态角、第二姿态角和位移之前，初始化上述双目相机采集的图像的特征点，通过初始化双目相机的参数、惯性测量单元的参数以及双目相机采集的图像的特征点，提高了视觉定位算法的定位精度，具体包括双目相机小孔成像模型参数，惯性测量单元器件误差模型参数等其它相关系数初始化。

本申请的另一种实施例，第二初始化单元包括第一获取模块，第一获取模块用于获取上述双目相机采集的第一帧图像的特征点和特征点深度值，利用LK光流法，将第一帧图像用作特征点初始化，保证了双目视觉惯性里程计估计位姿的准确性。

本申请的又一种实施例，第一获取模块包括提取子模块、获取子模块、矫正子模块和计算子模块，提取子模块用于提取上述双目相机采集到的第一帧图像的上述左图像的第一特征点并保存上述第一特征点；获取子模块用于在上述第一帧图像的上述右图像上跟踪上述左图像的上述第一特征点，获取第二特征点，并保存上述第二特征点，上述第一特征点和上述第二特征点构成特征点对；矫正子模块用于对上述特征点对进行畸变矫正；计算子模块用于采用矫正后的上述特征点对，计算上述特征点对的上述特征点深度值，具体地，第一帧图像用作特征点初始化，具体通过对左图提取Shi-Tomas角点，保存特征点；利用LK光流法在右图上跟踪上一步骤在左图上提取的特征点，保存跟踪点对；根据径向畸变模型和切向畸变模型对特征点对进行畸变校正；通过左右图像对应的特征点对三角化计算特征点深度值，特征点深度值的计算公式为：z＝fb/d，其中z为特征点深度值，f为相机焦距，b为双目基线长度，d为两对应特征点的像素点差(视差)，通过获取第一帧图像的特征点和特征点深度值，保证了双目视觉惯性里程计估计位姿的准确性。

本申请的另一种实施例，第一获取单元包括第二获取模块、第三获取模块和第四获取模块，第二获取模块用于在初始化上述双目相机采集的图像的特征点之后，获取第N帧图像和第N+1帧图像，N为大于1的整数；第三获取模块用于获取第一时刻和第二时刻，上述第一时刻为获取上述第N帧图像的时刻，上述第二时刻为获取上述第N+1帧图像的上述时刻；第四获取模块用于获取处于第三时刻的上述第一姿态角，上述第三时刻在上述第一时刻和上述第二时刻之间，通过获取相邻的两帧图像之间的第一姿态角，即根据前后两帧图像的时间戳，提取在两帧图像间的惯性测量单元的测量值，保存；再根据惯性测量单元给出的测量值，通过四阶拢格库塔法更新姿态矩阵，进而得到准确的第一姿态角，进而保证了双目视觉惯性里程计估计位姿的准确性。

本申请的再一种实施例，第一获取单元还包括第五获取模块、第六获取模块、矫正模块和第七获取模块，第五获取模块用于在初始化上述双目相机采集的图像的特征点之后，获取第三特征点，上述第三特征点为上述第N帧图像的左图像上的特征点；第六获取模块用于在上述第N+1帧图像的左图像上跟踪上述第三特征点，获取第四特征点，上述第四特征点为上述第N+1帧图像的左图像上的特征点；矫正模块用于在上述第三特征点的数量大于第一预定值的情况下，对上述第三特征点和上述第四特征点进行畸变矫正；第七获取模块用于采用矫正后的上述第三特征点和上述第四特征点，获取上述第二姿态角和上述位移，通过第三特征点和第四特征点，得到了准确的第二姿态角和位移，进而保证了双目视觉惯性里程计估计位姿的准确性，上述第一预定值可以为10、20或30等，且通过对图像特征点的重复跟踪使用，降低了图像特征点重新提取时间，降低了视觉里程计定位误差累积。

本申请的一种实施例，位姿包括上述第二姿态角和上述位移，第一获取单元还包括比较模块、检测模块、替换模块和第八获取模块，比较模块用于比较上述第N+1帧图像的上述位姿与上述第N帧图像的上述位姿，得到比较结果，N为大于1的整数；检测模块用于根据上述比较结果检测是否有异常值；替换模块用于在有上述异常值的情况下，使用上述第N帧图像的上述位姿替换上述第N+1帧图像的上述位姿，且删除上述第N+1帧图像的上述位姿，并重新跟踪第N+M帧图像，M为大于等于2的整数；第八获取模块用于在没有上述异常值的情况下，根据第N+M帧图像的上述位姿获取上述第二姿态角和上述位移，利用2D-3D点对进行位姿的求解，其中使用RANSC(随机采样一致性算法)剔除掉误匹配特征点；计算当前帧图像的位姿(R，T)与上一帧图像的位姿(R，T)进行对比，检测是否为异常值，假如为异常值则用上一帧图像位姿替换为当前帧图像的位姿，删除当前帧图像的相关特征点，并返回重新跟踪下一帧图像，进一步地保证了第二姿态角和位移的准确性，进而保证了双目视觉惯性里程计估计位姿的准确性，具体地，异常值表示上述第N+1帧图像的上述位姿与上述第N帧图像的上述位姿的差值较大，由于载体一般是连续运动的，差值较大证明第N+1帧图像的上述位姿有较大的偏差，例如，比如用户在开车的过程中，GPS在10：00时候定位离终点还有1000公里(相当于第N帧图像)，那么GPS在10：10时候(相当于第N+1帧图像)定位离终点还差100公里，这1000-100＝900公里，这就“异常”，因为10分钟行驶900公里是违背常规的。

本申请的一种实施例，第一获取单元还用于设置时间阈值；在采集上述第一姿态角的时间达到上述时间阈值的情况下，采用上一次获取的上述第一姿态角作为惯性测量单元姿态更新的初值；根据上述初值更新上述第一姿态角，为了降低惯性器件的累积误差，根据惯性器件性能，设定时间阈值，当达到时间阈值则利用上一步获取的姿态设为惯性测量单元姿态更新的初值，进而保证了获取的第一姿态角的准确性，进而保证了双目视觉惯性里程计估计位姿的准确性。

本申请的一种实施例，更新单元还用于根据上述融合姿态角，在固定姿态矩阵的情况下，利用预定数量的2D-3D点对更新上述位移，单纯视觉里程计计算的姿态和位移，是由点对最小投影误差优化得到，因此当姿态出现误差，势必导致位移测量存在误差，通过本发明固定融合后的姿态角，再去进行位移值的求解算法，很大程度降低了载体位移轨迹偏差，利用融合优化后姿态角，挑选前10个2D-3D点对在固定姿态矩阵的情况下，求解位移值，达到优化视觉里程计的位置效果，进而保证了双目视觉惯性里程计估计位姿的准确性，当然，实际应用中可以选择20个2D-3D点对、30个2D-3D点对以及40个2D-3D点对等。

本申请的一种实施例，第一获取模块还用于获取第N帧图像正在跟踪的上述特征点的数量，N为大于1的整数；在上述特征点的数量小于第二预定值的情况下，增加上述特征点和对应的上述特征点深度值，即需要征点的数量达到一定数值的情况下，才能保证初始化双目相机采集的图像的特征点的准确性，进而保证双目视觉惯性里程计估计位姿的准确性，上述第二预定值可以为10、20或30等。

上述基于双目视觉惯性里程计估计位姿的装置包括处理器和存储器，上述第一获取单元、第二获取单元和更新单元等均作为程序单元存储在存储器中，由处理器执行存储在存储器中的上述程序单元来实现相应的功能。

处理器中包含内核，由内核去存储器中调取相应的程序单元。内核可以设置一个或以上，通过调整内核参数来提高视觉定位算法的定位精度。

存储器可能包括计算机可读介质中的非永久性存储器，随机存取存储器(RAM)和/或非易失性内存等形式，如只读存储器(ROM)或闪存(flash RAM)，存储器包括至少一个存储芯片。

本发明实施例提供了一种存储介质，其上存储有程序，该程序被处理器执行时实现上述基于双目视觉惯性里程计估计位姿的方法。

本发明实施例提供了一种处理器，上述处理器用于运行程序，其中，上述程序运行时执行上述基于双目视觉惯性里程计估计位姿的方法。

本发明实施例提供了一种设备，设备包括处理器、存储器及存储在存储器上并可在处理器上运行的程序，处理器执行程序时实现至少以下步骤：

步骤S101，获取第一姿态角、第二姿态角和位移，上述第一姿态角为通过惯性测量单元获取的载体的姿态角，从双目视觉里程计获取的对应于上述载体的多帧图像中获取上述第二姿态角和上述位移；

步骤S102，获取上述第一姿态角和上述第二姿态角的加权平均值，上述加权平均值为上述载体的融合姿态角；

步骤S103，根据融合姿态角更新位移。

本文中的设备可以是服务器、PC、PAD、手机等。

本申请还提供了一种计算机程序产品，当在数据处理设备上执行时，适于执行初始化有至少如下方法步骤的程序：

步骤S101，获取第一姿态角、第二姿态角和位移，上述第一姿态角为通过惯性测量单元获取的载体的姿态角，从双目视觉里程计获取的对应于上述载体的多帧图像中获取上述第二姿态角和上述位移；

步骤S102，获取上述第一姿态角和上述第二姿态角的加权平均值，上述加权平均值为上述载体的融合姿态角；

步骤S103，根据融合姿态角更新位移。

本领域内的技术人员应明白，本申请的实施例可提供为方法、系统、或计算机程序产品。因此，本申请可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。而且，本申请可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器、CD-ROM、光学存储器等)上实施的计算机程序产品的形式。

本申请是参照根据本申请实施例的方法、设备(系统)、和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器，使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。

这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中，使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品，该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。

这些计算机程序指令也可装载到计算机或其他可编程数据处理设备上，使得在计算机或其他可编程设备上执行一系列操作步骤以产生计算机实现的处理，从而在计算机或其他可编程设备上执行的指令提供用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的步骤。

在一个典型的配置中，计算设备包括一个或多个处理器(CPU)、输入/输出接口、网络接口和内存。

存储器可能包括计算机可读介质中的非永久性存储器，随机存取存储器(RAM)和/或非易失性内存等形式，如只读存储器(ROM)或闪存(flash RAM)。存储器是计算机可读介质的示例。

计算机可读介质包括永久性和非永久性、可移动和非可移动媒体可以由任何方法或技术来实现信息存储。信息可以是计算机可读指令、数据结构、程序的模块或其他数据。计算机的存储介质的例子包括，但不限于相变内存(PRAM)、静态随机存取存储器(SRAM)、动态随机存取存储器(DRAM)、其他类型的随机存取存储器(RAM)、只读存储器(ROM)、电可擦除可编程只读存储器(EEPROM)、快闪记忆体或其他内存技术、只读光盘只读存储器(CD-ROM)、数字多功能光盘(DVD)或其他光学存储、磁盒式磁带，磁带磁磁盘存储或其他磁性存储设备或任何其他非传输介质，可用于存储可以被计算设备访问的信息。按照本文中的界定，计算机可读介质不包括暂存电脑可读媒体(transitory media)，如调制的数据信号和载波。

还需要说明的是，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、商品或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、商品或者设备所固有的要素。在没有更多限制的情况下，由语句“包括一个……”限定的要素，并不排除在包括要素的过程、方法、商品或者设备中还存在另外的相同要素。

实施例

本实施例涉及一种具体的基于双目视觉惯性里程计估计位姿的方法，如图3所示，为基于双目视觉惯性里程计估计位姿的方法的流程图，具体包括如下步骤：

1)双目相机小孔成像模型参数，惯性测量单元器件误差模型参数等其它相关系数初始化；

2)采集双目相机在左右图像，惯性测量单元的原始输出；

3)第一帧图像用作特征点初始化，具体通过对左图提取Shi-Tomas角点，保存特征点；

4)利用LK光流法在右图上跟踪上一步骤在左图上提取的特征点，保存跟踪点对；

5)根据径向畸变模型和切向畸变模型对特征点对进行畸变校正；

6)通过左右图像对应的特征点对三角化计算深度值，其中z为特征点深度值，f为相机焦距，b为双目基线长度，d为两对应特征点的像素点差(视差)：

z＝fb/d

7)特征点初始化结束后，继续采集下一帧图像(包含左右图像)；

8)根据前后两帧图像的时间戳，提取在两帧图像间的惯性测量单元的测量值，保存；

9)根据惯性测量单元给出的测量值，通过四阶拢格库塔法更新姿态矩阵；

10)利用LK光流法在当前帧左图上跟踪上一帧的左图特征点；

11)当跟踪后的特征点个数大于10个，继续下一步，否则返回，并输出提示，重新采集图像跟踪；

12)重复第5步骤；

13)利用2D-3D点对进行位姿的求解，其中使用RANSC(随机采样一致性算法)剔除掉误匹配特征点；

14)计算当前帧图像的位姿(R，T)与上一帧图像的位姿(R，T)进行对比，检测是否为异常值，假如为异常值则用上一帧图像位姿替换为当前帧图像的位姿，删除当前帧图像的相关特征点，并返回重新跟踪下一帧图像；

15)通过设定合适的视觉里程计解算的姿态角权值weightVo和惯性测量单元更新得到的姿态角权值weightImu，加权融合两者的姿态角，并将融合后的姿态替换上一步图像获取的姿态角其中，attitude表示融合姿态角，attitudeVo表示通过视觉里程计获取的第二姿态角，weightVo表示第二姿态角的比重，attitudeImu表示通过惯性测量单元获取的第一姿态角，weightImu表示第一姿态角的比重：

attitude＝weightVo×attitudeVo+weightImu×attitudeImu

16)为了降低惯性器件的累积误差，根据惯性器件性能，设定时间阈值，当达到时间阈值则利用上一步获取的姿态设为惯性测量单元姿态更新的初值；

17)利用融合优化后姿态，挑选前10个2D-3D点对在固定姿态矩阵的情况下，求解位移值，达到优化视觉里程计的位置效果；

18)判断当前帧正确跟踪的特征点数量，当大于设定的阈值，将不需要提取新的特征点，当小于设定的阈值，则重复步骤3，4，5，6增加新的特征点及对应深度值；

本实施例涉及一种具体的基于双目视觉惯性里程计估计位姿的方法对应的算法原理，如图4所示，为一种具体的基于双目视觉惯性里程计估计位姿的方法的算法原理图，具体包括：参数初始化、对第一帧图像的处理以及对非首帧图像的处理，参数初始化包括初始化双目相机的参数和初始化惯性测量单元的参数，对第一帧图像的处理左右图匹配、畸变矫正以及三角化计算特征点深度值，对非首帧图像的处理包括帧间匹配、畸变矫正、双目视觉里程计位姿求解，以及获取帧间IMU测量值、IMU位姿求解，将双目视觉里程计获取的位姿与IMU获取的位姿进行融合。

该方案中，通过惯性测量单元获取的载体的第一姿态角，通过双目视觉里程计获取载体的第二姿态角和位移，进而获取第一姿态角和第二姿态角的加权平均值，再根据融合姿态角新载体的位移，单纯的视觉里程计容易受到外界干扰(比如动态特征点)，将导致姿态角出现较大误差，通过将惯性测量单元和双目视觉里程计获取的姿态角进行融合，提升了融合系统的鲁棒性，降低了融合算法运算时间，使得融合算法调试更灵活，视觉里程计的姿态角的精度得到明显提升，尤其在旋转运动中，单纯视觉里程计容易出现比较大的旋转测量误差，而惯性测量单元表现稳定，两者通过融合提高了单纯的视觉定位算法的定位精度，单纯视觉里程计和惯性测量单元均存在累积误差，两者通过本发明算法融合，累积误差有所降低。

且本发明视觉惯性里程计算法代码能实时运行在嵌入式ARM9平台上，相比与目前采用优化算法的视觉惯性里程计，本发明运算资源达到嵌入式平台运算要求。

从以上的描述中，可以看出，本申请上述的实施例实现了如下技术效果：

1)、本申请的基于双目视觉惯性里程计估计位姿的方法，通过惯性测量单元获取的载体的第一姿态角，通过双目视觉里程计获取载体的第二姿态角和位移，进而获取第一姿态角和第二姿态角的加权平均值，再根据融合姿态角新载体的位移，单纯的视觉里程计容易受到外界干扰(比如动态特征点)，将导致姿态角出现较大误差，通过将惯性测量单元和双目视觉里程计获取的姿态角进行融合，提升了融合系统的鲁棒性，降低了融合算法运算时间，使得融合算法调试更灵活，视觉里程计的姿态角的精度得到明显提升，尤其在旋转运动中，单纯视觉里程计容易出现比较大的旋转测量误差，而惯性测量单元表现稳定，两者通过融合提高了单纯的视觉定位算法的定位精度，单纯视觉里程计和惯性测量单元均存在累积误差，两者通过本发明算法融合，累积误差有所降低，从而提高了视觉定位算法的定位精度。

2)、本申请的基于双目视觉惯性里程计估计位姿的装置，第一获取单元通过惯性测量单元获取的载体的第一姿态角，通过双目视觉里程计获取载体的第二姿态角和位移，第二获取单元获取第一姿态角和第二姿态角的加权平均值，更新单元根据融合姿态角新载体的位移，单纯的视觉里程计容易受到外界干扰(比如动态特征点)，将导致姿态角出现较大误差，通过将惯性测量单元和双目视觉里程计获取的姿态角进行融合，提升了融合系统的鲁棒性，降低了融合算法运算时间，使得融合算法调试更灵活，视觉里程计的姿态角的精度得到明显提升，尤其在旋转运动中，单纯视觉里程计容易出现比较大的旋转测量误差，而惯性测量单元表现稳定，两者通过融合提高了单纯的视觉定位算法的定位精度，单纯视觉里程计和惯性测量单元均存在累积误差，两者通过本发明算法融合，累积误差有所降低，从而提高了视觉定位算法的定位精度。

以上所述仅为本申请的优选实施例而已，并不用于限制本申请，对于本领域的技术人员来说，本申请可以有各种更改和变化。凡在本申请的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本申请的保护范围之内。

Claims (13)

1.一种基于双目视觉惯性里程计估计位姿的方法，其特征在于，包括：

获取第一姿态角、第二姿态角和位移，所述第一姿态角为通过惯性测量单元获取的载体的姿态角，从双目视觉里程计获取的对应于所述载体的多帧图像中获取所述第二姿态角和所述位移；

获取所述第一姿态角和所述第二姿态角的加权平均值，所述加权平均值为所述载体的融合姿态角；

根据所述融合姿态角更新所述位移。

2.根据权利要求1所述的方法，其特征在于，所述双目视觉里程计包括双目相机，在获取第一姿态角、第二姿态角和位移之前，所述方法还包括：

初始化所述双目相机的参数和所述惯性测量单元的参数；

采集所述双目相机的左图像和右图像，采集所述惯性测量单元的原始输出；

初始化所述双目相机采集的图像的特征点。

3.根据权利要求2所述的方法，其特征在于，初始化所述双目相机采集的图像的特征点，包括：

获取所述双目相机采集的第一帧图像的特征点和特征点深度值。

4.根据权利要求3所述的方法，其特征在于，获取所述双目相机采集的第一帧图像的特征点和特征点深度值，包括：

提取所述双目相机采集到的第一帧图像的所述左图像的第一特征点并保存所述第一特征点；

在所述第一帧图像的所述右图像上跟踪所述左图像的所述第一特征点，获取第二特征点，并保存所述第二特征点，所述第一特征点和所述第二特征点构成特征点对；

对所述特征点对进行畸变矫正；

采用矫正后的所述特征点对，计算所述特征点对的所述特征点深度值。

5.根据权利要求4所述的方法，其特征在于，在初始化所述双目相机采集的图像的特征点之后，获取第一姿态角包括：

获取第N帧图像和第N+1帧图像，N为大于1的整数；

获取第一时刻和第二时刻，所述第一时刻为获取所述第N帧图像的时刻，所述第二时刻为获取所述第N+1帧图像的所述时刻；

获取处于第三时刻的所述第一姿态角，所述第三时刻在所述第一时刻和所述第二时刻之间。

6.根据权利要求5所述的方法，其特征在于，在初始化所述双目相机采集的图像的特征点之后，获取第二姿态角和位移包括：

获取第三特征点，所述第三特征点为所述第N帧图像的左图像上的特征点；

在所述第N+1帧图像的左图像上跟踪所述第三特征点，获取第四特征点，所述第四特征点为所述第N+1帧图像的左图像上的特征点；

在所述第三特征点的数量大于第一预定值的情况下，对所述第三特征点和所述第四特征点进行畸变矫正；

采用矫正后的所述第三特征点和所述第四特征点，获取所述第二姿态角和所述位移。

7.根据权利要求6所述的方法，其特征在于，位姿包括所述第二姿态角和所述位移，获取所述第二姿态角和所述位移，还包括：

比较所述第N+1帧图像的所述位姿与所述第N帧图像的所述位姿，得到比较结果，N为大于1的整数；

根据所述比较结果检测是否有异常值；

在有所述异常值的情况下，使用所述第N帧图像的所述位姿替换所述第N+1帧图像的所述位姿，且删除所述第N+1帧图像的所述位姿，并重新跟踪第N+M帧图像，M为大于等于2的整数；

在没有所述异常值的情况下，根据第N+M帧图像的所述位姿获取所述第二姿态角和所述位移。

8.根据权利要求5所述的方法，其特征在于，获取第一姿态角，还包括：

设置时间阈值；

在采集所述第一姿态角的时间达到所述时间阈值的情况下，采用上一次获取的所述第一姿态角作为惯性测量单元姿态更新的初值；

根据所述初值更新所述第一姿态角。

9.根据权利要求1所述的方法，其特征在于，根据所述融合姿态角更新所述位移，包括：

根据所述融合姿态角，在固定姿态矩阵的情况下，利用预定数量的2D-3D点对更新所述位移。

10.根据权利要求3所述的方法，其特征在于，获取所述双目相机采集的第一帧图像的特征点和特征点深度值，还包括：

获取第N帧图像正在跟踪的所述特征点的数量，N为大于1的整数；

在所述特征点的数量小于第二预定值的情况下，增加所述特征点和对应的所述特征点深度值。

11.一种基于双目视觉惯性里程计估计位姿的装置，其特征在于，包括：

第一获取单元，获取第一姿态角、第二姿态角和位移，所述第一姿态角为通过惯性测量单元获取的载体的姿态角，从双目视觉里程计获取的对应于所述载体的多帧图像中获取所述第二姿态角和所述位移；

第二获取单元，获取所述第一姿态角和所述第二姿态角的加权平均值，所述加权平均值为所述载体的融合姿态角；

更新单元，用于根据所述融合姿态角更新所述位移。

12.一种存储介质，其特征在于，所述存储介质包括存储的程序，其中，所述程序执行权利要求1至10中任意一项所述的方法。

13.一种处理器，其特征在于，所述处理器用于运行程序，其中，所述程序运行时执行权利要求1至10中任意一项所述的方法。

Images