适用于机器人的视觉和惯性融合的位姿估计方法及系统

技术领域

本发明涉及机器人视觉技术领域，特别涉及一种适用于机器人的视觉和惯性融合的位姿估计方法及系统。

背景技术

现阶段，机器人的视觉和惯性传感器被广泛应用于机器人的各领域，如拣选机器人、焊接机器人、蔬菜嫁接机器人、采摘机器人等，通过视觉和惯性传感器控制机器人到达环境中的指定位姿，进而完成所给定的作业任务。

基于视觉的算法能够很好地处理长时间内的变化，但不能正确地观察到突然的旋转速率且容易受到光照影响。另一方面，陀螺仪在短时间内可精确测量其轴线的角速度，但在较长的周期内漂移很大。因此，视觉和陀螺仪的结合是相辅相成的，并可提供更健壮的姿态估计能力。当连续视频帧之间的像素位移主要由摄像机旋转引起时，可实现陀螺仪零漂移补偿。但是，现有技术实现视觉和惯性融合还存在初始化，非线性、坐标和时间不一致等问题。

发明内容

本发明提供了一种适用于机器人的视觉和惯性融合的位姿估计方法及系统，以解决现有位姿估计方法不够准确的技术问题。

为解决上述技术问题，本发明提供了如下技术方案：

一方面，本发明提供了一种适用于机器人的视觉和惯性融合的位姿估计方法，该适用于机器人的视觉和惯性融合的位姿估计方法包括：

获取机器人的视觉传感器采集的图像，对相邻帧图像的点线特征进行检测；

根据相邻帧图像的点线特征的位置和姿态变化，确定所述视觉传感器的位姿变化，建立用于表示所述视觉传感器的位姿变化的视觉位姿模型；

利用惯性传感器检测所述视觉传感器的位姿数据，并根据所述惯性传感器检测的位姿数据建立用于表示所述视觉传感器的位姿变化的惯性位姿模型；其中，所述惯性传感器安装在所述视觉传感器上并与所述视觉传感器同轴系设置；

将所述视觉位姿模型和所述惯性位姿模型进行数据融合，得到融合位姿模型，以通过所述融合位姿模型对机器人的视觉传感器的位姿进行估计。

进一步地，所述对相邻帧图像的点线特征进行检测，包括：

对于前一帧图像，通过预设的点线特征提取方法检索图像中的灭点、地脚点和铅垂线，并记录检索到的灭点、地脚点和铅垂线起止点在图像中的坐标位置；其中，所述灭点指在透视投影中，不平行于投影面的平行线投影聚集到的点；所述地脚点指不同平面的地脚线的交点；所述铅垂线指垂直于地面的直线；

通过所述惯性传感器检测到的所述视觉传感器的位姿数据，估计相邻帧图像的姿态和位置的变化，并基于前一帧图像中的灭点、地脚点和铅垂线起止点在图像中的坐标位置，根据估计出的相邻帧图像的姿态和位置的变化，预测下一帧图像中的灭点、地脚点和铅垂线起止点在图像中的坐标位置；

基于对下一帧图像中的灭点、地脚点和铅垂线起止点在图像中的坐标位置的预测结果，采用局部搜索法在下一帧图像中检索灭点、地脚点和铅垂线，并记录检索到的灭点、地脚点和铅垂线起止点在图像中的坐标位置。

进一步地，所述惯性传感器包括陀螺仪和加速度计，所述陀螺仪用于检测所述视觉传感器的角速度，所述加速度计用于检测所述视觉传感器的线加速度；

所述通过所述惯性传感器检测到的所述视觉传感器的位姿数据，估计相邻帧图像的姿态和位置的变化，包括：

通过对所述陀螺仪和加速度计检测到的角速度和线加速度进行预积分，得到所述视觉传感器的姿态和位置变化，以估计相邻帧图像的姿态和位置的变化。

进一步地，所述基于对下一帧图像中的灭点、地脚点和铅垂线起止点在图像中的坐标位置的预测结果，采用局部搜索法在下一帧图像中检索灭点、地脚点和铅垂线，包括：

基于对下一帧图像中的灭点、地脚点和铅垂线起止点在图像中的坐标位置的预测结果，在下一帧图像中设计搜索框；并在所述搜索框内采用预设的点线特征搜索方法对下一帧图像中的灭点、地脚点和铅垂线进行局部搜索。

进一步地，所述利用惯性传感器检测所述视觉传感器的位姿数据，并根据所述惯性传感器检测的位姿数据建立用于表示所述视觉传感器的位姿变化的惯性位姿模型，包括：

定周期地利用所述视觉位姿模型计算所述视觉传感器的姿态和位置变化，并利用计算结果，对所述惯性传感器进行偏差标定；

利用已标定的惯性传感器检测所述视觉传感器的位姿数据，并根据惯性传感器检测的位姿数据建立用于表示所述视觉传感器的位姿变化的惯性位姿模型。

另一方面，本发明还提供了一种适用于机器人的视觉和惯性融合的位姿估计系统，该适用于机器人的视觉和惯性融合的位姿估计系统包括：

图像点线特征提取模块，用于获取机器人的视觉传感器采集的图像，对相邻帧图像的点线特征进行检测；

视觉位姿模型建立模块，用于根据相邻帧图像的点线特征的位置和姿态变化，确定所述视觉传感器的位姿变化，建立用于表示所述视觉传感器的位姿变化的视觉位姿模型；

惯性位姿模型建立模块，用于利用惯性传感器检测所述视觉传感器的位姿数据，并根据所述惯性传感器检测的位姿数据建立用于表示所述视觉传感器的位姿变化的惯性位姿模型；其中，所述惯性传感器安装在所述视觉传感器上并与所述视觉传感器同轴系设置；

模型融合模块，用于将所述视觉位姿模型和所述惯性位姿模型进行数据融合，得到融合位姿模型，以通过所述融合位姿模型对机器人的视觉传感器的位姿进行估计。

进一步地，所述图像点线特征提取模块具体用于：

对于前一帧图像，通过预设的点线特征提取方法检索图像中的灭点、地脚点和铅垂线，并记录检索到的灭点、地脚点和铅垂线起止点在图像中的坐标位置；其中，所述灭点指在透视投影中，不平行于投影面的平行线投影聚集到的点；所述地脚点指不同平面的地脚线的交点；所述铅垂线指垂直于地面的直线；

通过所述惯性传感器检测到的所述视觉传感器的位姿数据，估计相邻帧图像的姿态和位置的变化，并基于前一帧图像中的灭点、地脚点和铅垂线起止点在图像中的坐标位置，根据估计出的相邻帧图像的姿态和位置的变化，预测下一帧图像中的灭点、地脚点和铅垂线起止点在图像中的坐标位置；

基于对下一帧图像中的灭点、地脚点和铅垂线起止点在图像中的坐标位置的预测结果，采用局部搜索法在下一帧图像中检索灭点、地脚点和铅垂线，并记录检索到的灭点、地脚点和铅垂线起止点在图像中的坐标位置。

进一步地，所述惯性传感器包括陀螺仪和加速度计，所述陀螺仪用于检测所述视觉传感器的角速度，所述加速度计用于检测所述视觉传感器的线加速度；

所述图像点线特征提取模块具体还用于：

通过对所述陀螺仪和加速度计检测到的角速度和线加速度进行预积分，得到所述视觉传感器的姿态和位置变化，以估计相邻帧图像的姿态和位置的变化。

进一步地，所述图像点线特征提取模块具体还用于：

基于对下一帧图像中的灭点、地脚点和铅垂线起止点在图像中的坐标位置的预测结果，在下一帧图像中设计搜索框；并在所述搜索框内采用预设的点线特征搜索方法对下一帧图像中的灭点、地脚点和铅垂线进行局部搜索。

进一步地，所述惯性位姿模型建立模块具体用于：

定周期地利用所述视觉位姿模型计算所述视觉传感器的姿态和位置变化，并利用计算结果，对所述惯性传感器进行偏差标定；

利用已标定的惯性传感器检测所述视觉传感器的位姿数据，并根据惯性传感器检测的位姿数据建立用于表示所述视觉传感器的位姿变化的惯性位姿模型。

再一方面，本发明还提供了一种电子设备，其包括处理器和存储器；其中，存储器中存储有至少一条指令，所述指令由处理器加载并执行以实现上述方法。

又一方面，本发明还提供了一种计算机可读存储介质，所述存储介质中存储有至少一条指令，所述指令由处理器加载并执行以实现上述方法。

本发明提供的技术方案带来的有益效果至少包括：

1、针对现有技术点线特征朴素性、提取精度低和匹配速度慢的问题，本发明提出了具有独特性的灭点和地脚点作为点特征，灭线和铅垂线作为线特征的点线特征提取方法，包括：建立灭点评估模型，根据获取的灭线信息择优选取实现高精度的灭点坐标估计；建立点特征匹配模型，根据当前地脚点坐标位置，利用惯性传感器预估下一帧地脚点坐标的快速匹配算法；铅垂线择优选取与拟合模型，获取高精度的起止点坐标，基于此，本发明可提高匹配精度及速度。

2、本发明提出了基于点特征(灭点、铅垂线起止点、地脚点)建立视觉位姿模型，利用标定的惯性传感器采集的位姿数据，通过预积分建立惯性位姿模型，通过将二者采用多速率融合的方式进行融合，实现了一种高精度的位姿模型。

附图说明

为了更清楚地说明本发明实施例中的技术方案，下面将对实施例描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1是本发明实施例提供的适用于机器人的视觉和惯性融合的位姿估计方法的流程示意图；

图2是本发明实施例提供的前一帧点线特征在图像中位置计算的流程图；

图3是本发明实施例提供的后一帧点线特征在图像中位置计算的流程图；

图4是本发明实施例提供的惯性传感器位姿模型建立的流程图；

图5是本发明实施例提供的将视觉和惯性传感器数据融合获取高精度的位姿数据的流程示意图。

具体实施方式

为使本发明的目的、技术方案和优点更加清楚，下面将结合附图对本发明实施方式作进一步地详细描述。

第一实施例

本实施例提供了一种快速、准确、应用广泛并适用于运动载体的适用于机器人的视觉和惯性融合的位姿估计方法，该方法可以由电子设备实现，该电子设备可以是终端或者服务器。该方法的执行流程如图1所示，包括以下步骤：

S1，获取机器人视觉传感器采集的图像，对相邻帧图像点线特征进行检测；

S2，根据相邻帧图像的点线特征的位置和姿态变化，确定所述视觉传感器的位姿变化，建立用于表示所述视觉传感器的位姿变化的视觉位姿模型；

S3，利用惯性传感器检测视觉传感器的位姿数据，并根据所述惯性传感器检测的位姿数据建立用于表示所述视觉传感器的位姿变化的惯性位姿模型；其中，所述惯性传感器安装在所述视觉传感器上并与所述视觉传感器同轴系设置；

S4，将所述视觉位姿模型和所述惯性位姿模型进行数据融合，得到融合位姿模型，以通过所述融合位姿模型对机器人的视觉传感器的位姿进行估计。

进一步地，请参阅图2和图3，上述S1的实现过程具体如下：

S11，获取机器人视觉传感器采集的图像；

S12，对于前一帧图像，通过点线特征提取方法检索图像中的灭点、地脚点和铅垂线，并记录检索到的灭点、地脚点和铅垂线起止点在图像中的坐标位置；

其中，所述灭点指在透视投影中，不平行于投影面的平行线投影聚集到的点；所述地脚点指不同平面的地脚线的交点；所述铅垂线指垂直于地面的直线；

本实施例所采用的点线特征提取方法为：采用霍夫变换（Hough）和直线检测分割算法（LSD）提取图像中点线特征，如图2所示，其具体流程步骤如下：

对获取的原始图像进行Hough变换，提取相关平行线，计算线段斜率；

通过LSD算法计算线段斜率和线段交点，根据线段交点确定地脚点，并估计灭点，根据计算的线段斜率确定铅垂线；

根据Hough变换计算的线段斜率和LSD算法计算的线段斜率建立灭点评价函数，对估计的灭点进行评价，获取灭点信息；

记录灭点、地脚点和铅垂线起止点在图像中的坐标位置。

灭点提取的最优模型如下：

式中，

S13，通过所述惯性传感器检测到的所述视觉传感器的位姿数据，估计相邻帧图像的姿态和位置的变化，并基于前一帧图像中的灭点、地脚点和铅垂线起止点在图像中的坐标位置，根据估计出的相邻帧图像的姿态和位置的变化，预测下一帧图像中的灭点、地脚点和铅垂线起止点在图像中的坐标位置；

其中，在本实施例中，惯性传感器包括陀螺仪和加速度计，陀螺仪用于检测视觉传感器的角速度，加速度计用于检测视觉传感器的线加速度；

上述的通过所述惯性传感器检测到的所述视觉传感器的位姿数据，估计相邻帧图像的姿态和位置的变化，如图3所示，具体如下：

通过对所述陀螺仪和加速度计检测到的角速度和线加速度进行预积分，得到所述视觉传感器的姿态和位置变化，以估计相邻帧图像的姿态和位置的变化。

S14，基于对下一帧图像中的灭点、地脚点和铅垂线起止点在图像中的坐标位置的预测结果，采用局部搜索法在下一帧图像中检索灭点、地脚点和铅垂线，并记录检索到的灭点、地脚点和铅垂线起止点在图像中的坐标位置。

其中，在本实施例中，基于对下一帧图像中的灭点、地脚点和铅垂线起止点在图像中的坐标位置的预测结果，采用局部搜索法在下一帧图像中检索灭点、地脚点和铅垂线，如图3所示，具体如下：

基于对下一帧图像中的灭点、地脚点和铅垂线起止点在图像中的坐标位置的预测结果，在下一帧图像中设计5×5的搜索框；在所述搜索框内采用序贯相似性检测算法（SSDA）和随机抽样一致算法（RANSAC）对下一帧图像中的灭点、地脚点和铅垂线进行局部搜索。

进一步地，请参阅图4，上述S3的实现过程具体如下：

S31，定周期地利用所述视觉位姿模型计算所述视觉传感器的姿态和位置变化，并利用计算结果，对所述惯性传感器的陀螺仪和加速度计进行偏差标定；

S32，利用已标定的惯性传感器检测所述视觉传感器的位姿数据，并通过对所述陀螺仪和所述加速度计检测到的角速度和线加速度进行预积分，得到所述视觉传感器的姿态和位置变化，根据所述惯性传感器检测的所述视觉传感器的位姿变化数据，建立用于表示所述视觉传感器的位姿变化的惯性位姿模型。

进一步地，请参阅图5，上述S4的实现过程具体如下：

S41，通过所述视觉位姿模型获取视觉传感器的位姿数据；

S42，通过所述惯性位姿模型获取视觉传感器的位姿数据；

S43，将所述视觉位姿模型和所述惯性位姿模型对应的位姿数据采用多速率滤波算法进行数据融合，获取高质量的位姿估计结果。

综上，本实施例的方法通过获取视觉传感器输出的图像，对相邻帧图像的点线特征进行检测；根据相邻帧图像的点线特征的位姿变化，建立视觉位姿模型；利用惯性传感器检测视觉传感器的位姿数据，建立惯性位姿模型；将视觉位姿模型和惯性位姿模型进行数据融合，通过融合位姿模型对机器人的视觉传感器的位姿进行估计。解决了视觉传感器受光线和环境遮挡丢帧无法估计位姿、惯性传感器长时间运动零偏误差累积以及单一传感器位姿估计精度低的问题，改善了现实环境下位姿估计精度和鲁棒性，可广泛应用于机器人环境感知领域。

第二实施例

本实施例提供了一种适用于机器人的视觉和惯性融合的位姿估计系统，该适用于机器人的视觉和惯性融合的位姿估计系统包括以下模块：

图像点线特征提取模块，用于获取机器人的视觉传感器采集的图像，对相邻帧图像的点线特征进行检测；

视觉位姿模型建立模块，用于根据相邻帧图像的点线特征的位置和姿态变化，确定所述视觉传感器的位姿变化，建立用于表示所述视觉传感器的位姿变化的视觉位姿模型；

惯性位姿模型建立模块，用于利用惯性传感器检测所述视觉传感器的位姿数据，并根据所述惯性传感器检测的位姿数据建立用于表示所述视觉传感器的位姿变化的惯性位姿模型；其中，所述惯性传感器安装在所述视觉传感器上并与所述视觉传感器同轴系设置；

模型融合模块，用于将所述视觉位姿模型和所述惯性位姿模型进行数据融合，得到融合位姿模型，以通过所述融合位姿模型对机器人的视觉传感器的位姿进行估计。

本实施例的适用于机器人的视觉和惯性融合的位姿估计系统与上述第一实施例的适用于机器人的视觉和惯性融合的位姿估计方法相对应；其中，本实施例的适用于机器人的视觉和惯性融合的位姿估计系统中的各功能模块所实现的功能与上述第一实施例的适用于机器人的视觉和惯性融合的位姿估计方法中的各流程步骤一一对应；故，在此不再赘述。

第三实施例

本实施例提供一种电子设备，其包括处理器和存储器；其中，存储器中存储有至少一条指令，所述指令由处理器加载并执行，以实现第一实施例的方法。

该电子设备可因配置或性能不同而产生比较大的差异，可以包括一个或一个以上处理器（central processing units，CPU）和一个或一个以上的存储器，其中，存储器中存储有至少一条指令，所述指令由处理器加载并执行上述方法。

第四实施例

本实施例提供一种计算机可读存储介质，该存储介质中存储有至少一条指令，所述指令由处理器加载并执行，以实现上述第一实施例的方法。其中，该计算机可读存储介质可以是ROM、随机存取存储器、CD-ROM、磁带、软盘和光数据存储设备等。其内存储的指令可由终端中的处理器加载并执行上述方法。

此外，需要说明的是，本发明可提供为方法、装置或计算机程序产品。因此，本发明实施例可采用完全硬件实施例、完全软件实施例或结合软件和硬件方面的实施例的形式。而且，本发明实施例可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质上实施的计算机程序产品的形式。

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

这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理终端设备以特定方式工作的计算机可读存储器中，使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品，该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。这些计算机程序指令也可装载到计算机或其他可编程数据处理终端设备上，使得在计算机或其他可编程终端设备上执行一系列操作步骤以产生计算机实现的处理，从而在计算机或其他可编程终端设备上执行的指令提供用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的步骤。

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

最后需要说明的是，以上所述是本发明优选实施方式，应当指出，尽管已描述了本发明优选实施例，但对于本技术领域的技术人员来说，一旦得知了本发明的基本创造性概念，在不脱离本发明所述原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。所以，所附权利要求意欲解释为包括优选实施例以及落入本发明实施例范围的所有变更和修改。