CN113706620B - 一种基于参照对象的定位方法、定位装置、可移动平台 - Google Patents

Abstract

一种基于参照对象的定位方法、定位装置、可移动平台，涉及机器人技术领域。定位方法包括：获取第一图像，基于第一图像提取得到第一特征；获取第一位姿与参照对象所处平面的相对距离；获取第一位姿下采集装置的采集方向与参照对象的相对角度。获取第二图像，基于第二图像提取得到第二特征。基于第一特征和第二特征进行匹配计算，得到第一位姿与第二位姿的相对位姿关系。基于相对位姿关系，相对距离和相对角度计算，得到第二位姿与参照对象的相对位姿关系。定位装置包括获取模块、提取模块、数据计算模块、存储模块。可移动平台包括采集装置，移动装置、移动控制装置、定位装置。本发明定位受外界环境影响小，整体占据计算资源少，定位快速。

Description

一种基于参照对象的定位方法、定位装置、可移动平台

技术领域

本发明涉及空间精密测量技术领域，尤其涉及一种基于参照对象的定位方法、定位装置、可移动平台。

背景技术

近年来现有采用标识定位的方法：一种是采用二维码定位，需要获取二维码的先验信息（尺寸、图样）；还有一种是采用任意图像定位，需要预先建图，做全局优化，采用的是三维点信息。第一种方法需要获取二维码的先验信息，需要很多的预先数据获取和处理，耗费时间，且在二维码较多时需要的存储空间较大，耗费成本较大。第二种方法中的预先建图和全局优化都需要大量的准备时间，且到不同的环境都需要重新建图，而三维点信息的计算步骤较为复杂。

发明内容

本发明针对上述现有技术存在的问题，提供一种基于参照对象的定位方法。

本发明涉及一种基于参照对象的定位方法，用于设有采集装置的可移动平台，包括如下步骤：

获取第一图像，所述的第一图像基于采集装置位于第一位姿采集参照对象所得；基于所述的第一图像提取特征，得到第一特征；同时获取所述的第一位姿与参照对象所处平面的相对距离；以及获取所述第一位姿下采集装置的采集方向与所述参照对象的相对角度；

获取第二图像，所述的第二图像基于采集装置位于第二位姿采集参照对象所得；基于所述的第二图像提取特征，得到第二特征；

基于所述的第一特征和所述的第二特征进行匹配计算，得到所述第一位姿与所述第二位姿的相对位姿关系；

基于所述第一位姿与所述第二位姿的相对位姿关系，所述的相对距离和所述的相对角度计算，得到所述的第二位姿与所述的参照对象的相对位姿关系。

本发明不需要预先建图，可以直接通过第一位姿下采集参照对象的第一图像的特征，以及第一位姿下的相对距离和相对角度作为参照，而后续采集的图像均可以通过计算获得与所述的参照对象的相对位姿关系，受外界环境影响小，整体计算数据少，计算快速，提高了定位速度和定位适应性。

作为优选，所述第一位姿为采集装置的采集方向垂直于所述参照对象的位姿。

作为优选，所述的获取第一图像，所述的第一图像基于采集装置位于第一位姿采集参照对象所得之前，还包括如下步骤：检测所述采集装置当前位姿与第一位姿的偏差后，调整所述采集装置到第一位姿。

作为优选，所述的检测所述采集装置当前位姿与第一位姿的偏差后，调整所述采集装置到第一位姿，具体包括如下步骤：

重复如下步骤，直至实际偏移角度处于预设阈值范围之内：

获得所述采集装置的第一测距点和第二测距点在当前位姿下，分别距离所述参照对象的第一当前位姿距离和第二当前位姿距离；通过所述第一当前位姿距离、所述第二当前位姿距离与第一测距点和第二测距点的间距计算实际偏移角度；判断所述实际偏移角度与预设偏移角度的大小，得到判断结果一；判断所述第一当前位姿距离和所述第二当前位姿距离的大小，得到判断结果二；基于所述判断结果一和所述判断结果二调整可移动平台。

作为优选，所述的获取所述的第一位姿与参照对象所处平面的相对距离，包括如下步骤：获得所述采集装置的第一测距点和第二测距点在第一位姿下，分别距离所述参照对象的第一位姿距离和第二位姿距离，计算获得所述第一位姿距离和所述第二位姿距离的平均值为所述相对距离。

作为优选，所述的基于所述的第一图像提取特征，得到第一特征，包括如下步骤：通过特征提取算法基于所述的第一图像提取特征点，并通过特征提取算法计算获得第一图像特征描述子，集合得到所述第一特征。

作为优选，所述的基于所述的第二图像提取特征，得到第二特征，包括如下步骤：通过特征提取算法基于所述的第二图像提取特征点，并通过特征提取算法计算获得第二图像特征描述子，集合得到所述第二特征。

作为优选，所述的基于所述的第一特征和所述的第二特征进行匹配计算，得到所述第一位姿与所述第二位姿的相对位姿关系，包括如下步骤：通过Hamming距离对第一特征与第二特征进行关联匹配，得到匹配特征；结合RANSAC算法与采集装置的投影误差尺度，求解匹配特征的Homography变换，优化所述Homography变换得到所述第一位姿与所述第二位姿的相对位姿关系。

作为优选，所述的优化所述Homography变换得到所述第一位姿与所述第二位姿的相对位姿关系，包括如下步骤：将所述Homography变换分解为四个可能解，然后利用平面法向量先验约束得到旋转向量和平移向量。

作为优选，所述的基于所述第一位姿与所述第二位姿的相对位姿关系，所述的相对距离和所述的相对角度计算，得到所述的第二位姿与所述的参照对象的相对位姿关系，包括如下步骤：将所述的相对距离作用到所述第一位姿与所述第二位姿的相对位姿关系的平移向量中，从而得到真实平移距离；将所述的相对角度作用到所述第一位姿与所述第二位姿的相对位姿关系的旋转向量中，从而得到真实旋转角度；通过所述真实平移距离和真实旋转角度计算得到所述的第二位姿与所述的参照对象的相对位姿关系。

作为优选，若所述采集装置采集第二参照对象的第一图像后，基于所述第二参照对象的第一图像提取特征得到的第一特征，与所述采集装置采集第一参照对象的第一图像的第一特征相同，则获取所述的第一位姿与第二参照对象所处平面的相对距离时，直接获取第一位姿与第一参照对象所处平面的相对距离；以及获取所述第一位姿下第二采集装置的采集方向与所述参照对象的相对角度时，直接获取所述第一位姿下第一采集装置的采集方向与所述参照对象的相对角度。

作为优选，若基于所述的第二图像提取特征，得到的第二特征由多组第二特征构成，则选取一组第二特征与所述的第一特征进行匹配计算。

第二方面，本发明涉及一种定位装置，用于设有采集装置的可移动平台，包括：

获取模块，用于获取第一位姿下采集装置采集参照对象的第一图像，第二位姿下采集装置采集参照对象的第二图像，第一位姿下采集装置与参照对象所处平面的相对距离，第一位姿下采集装置的采集方向与所述参照对象的相对角度；

提取模块，用于基于所述的第一图像提取特征，得到第一特征；用于基于所述的第二图像提取特征，得到第二特征；

数据计算模块，用于基于所述第一特征，所述第二特征，所述的第一位姿下采集装置与参照对象所处平面的相对距离，所述的第一位姿下采集装置的采集方向与所述参照对象的相对角度，计算获得所述的第二位姿与所述的参照对象的相对位姿关系；

存储模块，用于存储所述第一特征及其对应的参照对象。

作为优选，所述定位装置还包括位姿调节模块，用于检测所述采集装置当前位姿与第一位姿的偏差后，调整所述采集装置到第一位姿，所述的第一位姿为采集装置的采集方向垂直于所述参照对象的位姿。

作为优选，所述位姿调节模块包括：

测量数据获取单元，用于获取所述采集装置的第一测距点和第二测距点在当前位姿下，分别距离所述参照对象的第一当前位姿距离和第二当前位姿距离；

偏移角度计算单元，用于基于所述第一当前位姿距离、所述第二当前位姿距离与第一测距点和第二测距点的间距计算实际偏移角度；

第一判断单元，用于判断所述实际偏移角度与预设偏移角度的大小，得到判断结果一；

第二判断单元，判断所述第一当前位姿距离和所述第二当前位姿距离的大小，得到判断结果二；

信号传输单元，基于所述判断结果一和所述判断结果二传输到可移动平台。

作为优选，所述数据计算模块包括：

第一计算单元，用于通过Hamming距离对第一特征与第二特征进行关联匹配，得到匹配特征；

第二计算单元，用于结合RANSAC算法与采集装置的投影误差尺度，求解匹配特征的Homography变换；

优化单元，用于优化所述Homography变换得到所述第一位姿与所述第二位姿的相对位姿关系；

第三计算单元，基于所述第一位姿与所述第二位姿的相对位姿关系，所述的相对距离和所述的相对角度计算，得到所述的第二位姿与所述的参照对象的相对位姿关系。

作为优选，所述定位装置还包括选择模块，用于在基于所述的第二图像提取特征，得到的第二特征由多组第二特征构成时，选取一组第二特征发送到获取模块。

第三方面，本发明涉及一种可移动平台，包括采集装置，移动装置、移动控制装置、所述的定位装置；所述采集装置与所述定位装置通信连接，所述移动控制装置与所述移动装置通信连接，所述定位装置与所述移动控制装置通信连接。

第四方面，本发明涉及一种计算机设备，包括存储器和处理器，所述存储器存储有计算机程序，所述处理器执行所述计算机程序时实现所述基于参照对象的定位方法的步骤。

第五方面，本发明涉及一种计算机可读存储介质，其上存储有计算机程序，所述计算机程序被处理器执行时实现所述基于参照对象的定位方法的步骤。

采用以上结构后，本发明与现有技术相比，具有以下优点：

本发明不需要预先建图，可以直接通过第一位姿下采集参照对象的第一图像的特征，第一位姿下的相对距离和相对角度作为参照，以及第二位姿下采集参照对象的第二图像的特征，计算获得所述的第二位姿与所述的参照对象的相对位姿关系，受外界环境影响小，整体计算数据少，计算快速，提高了定位速度和定位适应性。

附图说明

图1是本发明一种基于参照对象的定位方法一种实施例中的流程图。

图2是本发明一种定位装置一种实施例中的模块框图。

图3是本发明一种计算机设备一个实施例中的内部结构图。

图4是本发明一种基于参照对象的定位方法一种实施例中的位姿调节和距离测量俯视图。

图中：1-可移动平台，11-采集装置，12-第一测距点，13-第二测距点，2-参照对象，110-获取模块，120-提取模块，130-数据计算模块，140-存储模块。

具体实施方式

下面结合附图和具体实施例对本发明作进一步说明。

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。除非另有定义，本文所使用的所有的技术和科学术语与属于本发明的技术领域的技术人员通常理解的含义相同。本文中在本发明的说明书中所使用的术语只是为了描述具体的实施例的目的，不是旨在于限制本发明。

本发明实施例中所有方向性指示仅用于解释在某一特定姿态下各部件之间的相对位置关系，如果该特定姿态发生改变时，则该方向性指示也相应地随之改变。另外，在本发明中如涉及“第一”、“第二”等的描述仅用于描述目的，而不能理解为指示或暗示其相对重要性或者隐含指明所指示的技术特征的数量。由此，限定有“第一”、“第二”的特征可以明示或者隐含地包括至少一个该特征。

如图1所示，一种基于参照对象2的定位方法，用于设有采集装置11的可移动平台1，包括如下步骤：

步骤S01，获取第一图像，所述的第一图像基于采集装置11位于第一位姿采集参照对象2所得；基于所述的第一图像提取特征，得到第一特征；同时获取所述的第一位姿与参照对象2所处平面的相对距离；以及获取所述第一位姿下采集装置11的采集方向与所述参照对象2的相对角度。

位姿，即位置和姿态，是坐标系的位置和方向总称。

特征提取，在机器学习、模式识别和图像处理中，特征提取从初始的一组测量数据开始，并建立旨在提供信息和非冗余的派生值（特征），从而促进后续的学习和泛化步骤，并且在某些情况下带来更好的可解释性。

参照对象2，包括具有任意特征的标识，其特征可以被识别并从中提取。具体地，可以是具有任意形状、图案的标识。若标识为二维码，则可以是AprilTag或其他形式。标识也可以是二维码和其他图案、色彩的组合。而且标识不需要具有已知的位姿信息，例如二维码不需要具有先验信息，标识不需要建坐标系。

采集装置11，用于采集参照对象2。该采集装置11可以是相机，也可以是其他传感器。在本申请中，默认采集装置11的外参已被标定。采集装置11的外参为采集装置11与可移动平台1之间的坐标转换关系，可用以表示采集装置11位于可移动平台1上的位置和角度。采用单个相机位最优，是基于单目相机来采集图像，降低成本的同时，也减少在可移动平台1安装设置的工序。相对于采用多目相机，配置与标定较为简单，且消耗计算资源更少。

获取第一图像，所述的第一图像基于采集装置11位于第一位姿采集参照对象2所得。实际获取时，可以是采集装置11位于第一位姿直接采集参照对象2获取第一图像，也可以是直接调取之前存储的所述采集装置11位于第一位姿采集参照对象2获得的第一图像。

获取所述的第一位姿与参照对象2所处平面的相对距离，可以通过测距传感器进行测量，例如超声波测距传感器、激光测距传感器、红外线测距传感器等。也可以直接通过人工测量获取相对距离。

获取所述第一位姿下采集装置11的采集方向与所述参照对象2的相对角度，可以采用经纬仪或全站仪进行角度测量，也可以通过人工测量的相对距离和采集装置11实际与所述参照对象2的距离计算获得相对角度。

步骤S02，获取第二图像，所述的第二图像基于采集装置11位于第二位姿采集参照对象2所得；基于所述的第二图像提取特征，得到第二特征。

步骤S03，基于所述的第一特征和所述的第二特征进行匹配计算，得到所述第一位姿与所述第二位姿的相对位姿关系。

特征匹配，就是指将从影像中提取的特征作为共轭实体,而将所提特征属性或描述参数(实际上是特征的特征，也可以认为是影像的特征)作为匹配实体，通过计算匹配实体之间的相似性测度以实现共轭实体配准的影像匹配方法。

此处因为采用的是单目相机，所以所述相对位姿关系中没有距离的参数，所以自由度只有5个。

步骤S04，基于所述第一位姿与所述第二位姿的相对位姿关系，所述的相对距离和所述的相对角度计算，得到所述的第二位姿与所述的参照对象2的相对位姿关系。

本发明不需要预先建图，可以直接通过第一位姿下采集参照对象2的第一图像的特征，以及第一位姿下的相对距离和相对角度作为参照，而后续采集的图像均可以通过计算获得与所述的参照对象2的相对位姿关系，受外界环境影响小，整体计算数据少，计算快速，提高了定位速度和定位适应性。因为得到的数据是即时的，在可移动平台1和/或参照对象2移动的场景中，其定位精度高。

在具体实施过程中，所述的采集装置11固定安装于待定位的可移动平台1上，以单目相机为例加以说明；上述参照对象2可以设置于固定点位，也可以设置于其他移动体上。本方法以处理数据的定位装置视角为例加以说明。在前期，如步骤S01，待定位的可移动平台1的单目相机处于第一位姿，此时通过单目相机采集得到所述的第一图像，基于所述的第一图像提取特征，得到第一特征。同时通过人工或者传感器测量方式获取所述的相对距离和所述的相对角度。在待定位时刻，如步骤S02，待定位的可移动平台1的单目相机处于第二位姿，此时通过单目相机采集获取所述的第二图像，基于所述的第二图像提取特征，得到第二特征。如步骤S03，基于所述的第一特征和所述的第二特征进行匹配计算，得到所述第一位姿与所述第二位姿的相对位姿关系，此时的相对位姿关系为归一化尺度。最后如步骤S04，通过引入真实尺度，即所述的相对距离和所述的相对角度，进行尺度恢复得到真实物理尺寸。本技术可运用于多个可移动平台1的相互跟随。

一些实施例中，步骤S01中，所述第一位姿为采集装置11的采集方向垂直于所述参照对象2的位姿。这样设置使得可以直接获取所述第一位姿下采集装置11的采集方向与所述参照对象2的特征所处平面的相对角度为90度。同时获取所述的第一位姿与参照对象2所处平面的相对距离时，测量速度也可以得到提高，相对距离准确性也有所提高。

一些实施例中，所述第一位姿为采集装置11的采集方向垂直于所述参照对象2的位姿的情况下，所述的获取第一图像，所述的第一图像基于采集装置11位于第一位姿采集参照对象2所得之前，还包括如下步骤：检测所述采集装置11当前位姿与第一位姿的偏差后，调整所述采集装置11到第一位姿。调整所述采集装置11可以是通过控制可移动平台1自己转动，也可以是人工进行校正。

具体地，如图4所示，所述的检测所述采集装置22当前位姿与第一位姿的偏差后，调整所述采集装置11到第一位姿，具体包括如下步骤：

重复如下步骤，直至实际偏移角度处于预设阈值范围之内：

获得所述采集装置11的第一测距点12和第二测距点13在当前位姿下，分别距离所述参照对象2的第一当前位姿距离L1和第二当前位姿距离L2；通过所述第一当前位姿距离L1、所述第二当前位姿距离L2与第一测距点和第二测距点的间距L3计算实际偏移角度b；判断所述实际偏移角度b与预设偏移角度的大小，得到判断结果一。判断所述第一当前位姿距L1和和所述第二当前位姿距离L2的大小，得到判断结果二。基于所述判断结果一和所述判断结果二调整可移动平台。

其中，所述的第二测距点13不同于所述的第一测距点12，所述的第一测距点12和所述的第二测距点13位于所述的采集装置11上具有水平距离。所述的第一测距点12和所述的第二测距点13的连线平行与所述采集装置11的采集端表面。具体计算是：实际偏移角度b=arctan(第一当前位姿距离L1与第二当前位姿距离L2的差值/所述的第一测距点12和第二测距点13的间距L3），也可以分别计算水平分量。

若判断结果一是实际偏移角度大于预设偏移角度，判断结果二是所述第一当前位姿距离L1大于所述第二当前位姿距离L2，则将采集装置11对应所述第一测距点的一侧朝向参照对象2旋转实际偏移角度，然后重复上述步骤。若判断结果一是实际偏移角度大于预设偏移角度，判断结果二是所述第一当前位姿距离L1小于所述第二当前位姿距离L2，则将采集装置11对应所述第二测距点的一侧朝向参照对象2旋转实际偏移角度，然后重复上述步骤。若判断结果一是实际偏移角度小于预设偏移角度，无论判断结果二的情况如何，直接获取第一图像。这里预设偏移角度为0.5°。

调整第一位姿，使得相对角度的值可以得到保证，减少后续计算的误差。

一些实施例中，所述的获取所述的第一位姿与参照对象2所处平面的相对距离，包括如下步骤：获得所述采集装置的第一测距点12和第二测距点13在第一位姿下，分别距离所述参照对象2的第一位姿距离和第二位姿距离，计算获得所述第一位姿距离和所述第二位姿距离的平均值为所述相对距离。这样设置使得获取相对距离的值更准确，所述第一位姿距离和所述第二位姿距离的平均值可以减少掉原有的细微偏移角度所带来的误差值，而且可以在调整第一位姿时进行直接操作，简单快速。

一些实施例中，步骤S01中，所述的基于所述的第一图像提取特征，得到第一特征，包括如下步骤：通过特征提取算法基于所述的第一图像提取特征点，并通过特征提取算法计算获得第一图像特征描述子，集合得到所述第一特征。

可以采用不同的特征提取算法，特征提取算法包括但不限于FAST特征点检测算法、DOG特征点检测算法、Harris角点算法、SIFT特征点检测算法、SURF特征点检测算法等。

具体地，FAST（features from accelerated segment test）特征点检测算法，是公认的比较快速的特征点检测方法，只利用周围像素比较的信息就可以得到特征点，简单，有效。过程为选取一个像素点，即候选特征点后，检测候选特征点周围一圈像素点的灰度值，如果候选点周围领域内有足够多的像素点与该候选点的灰度值差别够大，则认为该候选点为一个特征点。具体公式如式（1）所示：

其中I（x）为候选特征点的周围一圈像素点的灰度值，I（p）为候选特征点的灰度值，Ed为灰度值差得阈值，如果N大于给定阈值，一般为周围一圈像素点的四分之三，则认为p是一个特征点。

具体地，DOG（Difference of Guassian）：简称高斯函数的差分，是灰度图像增强和角点检测的一种方法。先构建尺度空间，然后在不同参数下，将目标图像与高斯函数进行卷积运算得到目标图像的多个低通滤波结果，获得多个高斯滤波后的图片。通过两个相邻高斯尺度空间的图像相减，得到DOG的响应值图像。相减的图像包括原始的目标图像。通过获取的不同尺度下的DOG图，进而求角点，即依次求中间图片每个像素与该像素同尺度的8个相邻点以及上下相邻尺度对应的9*2共26个点的极值。一个点如果在DOG空间本层以及上下两层的26个领域中是最大值和最小值时，就认为该点是目标图像在该尺度下的一个特征点。

尺度空间就是在视觉信息处理模型中引入一个被视为尺度的参数，通过连续变化尺度参数获得不同尺度下的视觉处理信息，然后综合这些信息以深入地挖掘图像的本质特征。角点就是极值点，即在某方面属性特别突出的点，在此处就是特征点。

具体地，Harris（哈里斯）角点算法，原理是利用移动的窗口在图像中计算灰度变化值，其中关键流程包括转化为灰度图像、计算差分图像、高斯平滑、计算局部极值、确认角点。最后的角点就是特征值。

具体地，SIFT（Scale-invariant feature transform），也叫尺度不变特征变换算法。第一步，建立尺度空间，获得多个高斯滤波后的图片，然后进行尺度空间极值检测，即搜索所有尺度上的图像位置，这里Sift采用的是DOG图像。通过高斯微分函数来识别潜在的对于尺度和旋转不变的特征点。第二步，进行特征定位：在每个候选的位置上，通过一个拟合精细的模型来确定位置和尺度。第三步，方向确定：基于图像局部的梯度方向，分配给每个特征位置一个或多个方向。所有后面的对图像数据的操作都相对于关键点的方向、尺度和位置进行变换，从而提供对于这些变换的不变性。第四步，还可以计算生成SIFT描述子：先以特征点为中心，在附近邻域内将坐标轴旋转到与特征点的主方向相同，然后提取特征点周围4×4个区域块，统计每小块内8个梯度方向，这样一个关键点就可以产生128维的SIFT特征向量，作为SIFT特征描述子。

具体地，SURF（Speeded Up Robust Features）特征点检测算法，第一步，构建Hessian(黑塞矩阵)。第二步，构建尺度空间，获得Hessian矩阵行列式近似值图。第三步，进行特征点过滤并进行精确定位：将经过Hessian矩阵处理的每个像素点(即获得每个像素点Hessian矩阵的判别式值)与其图像域（相同大小的图像）和尺度域（相邻的尺度空间）的所有相邻点进行比较，当其大于（或者小于）所有相邻点时，该点就是极值点，即初步特征点。初步定位出特征点后，再经过滤除能量比较弱的关键点以及错误定位的关键点，筛选出最终的稳定的特征点。第四步，计算特征点主方向：SIFT算法特征点的主方向是采用在特征点邻域内统计其梯度直方图，横轴是梯度方向的角度，纵轴是梯度方向对应梯度幅值的累加，取直方图bin最大的以及超过最大80%的那些方向作为特征点的主方向。第五步，先以特征点为中心，在附近邻域内将坐标轴旋转到与特征点的主方向相同，然后提取特征点周围4×4个区域块，将水平方向值之和、垂直方向值之和、水平方向值绝对值之和以及垂直方向绝对之和4个方向作为每个子块区域的特征向量，所以一共有4×4×4=64维向量作为SURF特征描述子。

SIFT特征点检测算法、SURF特征点检测算法除了可以自己计算生成相应描述子。还可以根据brief算法，在所述关键点周围选取点对，通过比较像素值，生成brief特征描述子。FAST特征点检测算法、DOG特征点检测算法、Harris角点算法也可以在上述特征点确定后，通过brief算法生成brief特征描述子。

另外，一种实施方式,BRISK(Binary Robust Invariant Scalable Keypoints)也是一种基于二进制编码的特征描述子，对噪声鲁棒，具有尺度不变性和旋转不变性。给定一组特征点(包含亚像素图像位置和浮点型尺度值)，BRISK通过比较邻域Patch内像素点对的灰度值，并进行二进制编码得到BRISK特征描述子。通过计算具有长距离的采样点对的梯度来表示特征点的方向。

一种实施方式，FREAK（ Fast Retina Keypoint），FREAK也是一种基于二进制编码的图像特征描述子，计算较快，对噪声鲁棒，具有尺度不变性和旋转不变性。FREAK算法采用二进制编码描述特征点，过程中会对特征的描述向量进行筛选，也就是降维。然后计算特征点方向，采用距离长的、对称的采样点计算其梯度，据梯度进一步求得特征点的主方向。最后获得FREAK特征描述子。

一些实施例中，步骤S02中，所述的基于所述的第二图像提取特征，得到第二特征，包括如下步骤：通过特征提取算法基于所述的第二图像提取特征点，并通过特征提取算法计算获得第二图像特征描述子，集合得到所述第二特征。具体过程同上述获得第一特征的各实施方式，此处不再赘述。

一些实施例中，步骤S03中，所述的基于所述的第一特征和所述的第二特征进行匹配计算，得到所述第一位姿与所述第二位姿的相对位姿关系，包括如下步骤：通过Hamming距离对第一特征与第二特征进行关联匹配，得到匹配特征；结合RANSAC算法与采集装置的投影误差尺度，求解匹配特征的Homography变换，优化所述Homography变换得到所述第一位姿与所述第二位姿的相对位姿关系。

Hamming距离，即汉明距离，两个等长字符串之间的汉明距离是两个字符串对应位置的不同字符的个数。换句话说，它就是将一个字符串变换成另外一个字符串所需要替换的字符个数。

RANSAC （Random Sample Consensus）算法，即随机抽样一致算法，它是根据一组包含异常数据的样本数据集，计算出数据的数学模型参数，得到有效样本数据的算法。

Homography变换，即单应性变换，用于描述物体在世界坐标系和像素坐标系之间的位置映射关系。对应的变换矩阵称为单应性矩阵。

通过Hamming距离对第一特征与第二特征进行关联匹配，得到匹配特征，具体包括，计算第一特征的特征描述子的字符串和第二特征的特征描述子的字符串之间的汉明距离，例如，若汉明距离小于2则可以说两个特征描述子是相同的特征描述子；若汉明距离大于等于2，但小于5，则可以说两个特征描述子相互匹配，具体范围根据字符串的长度来定。

结合RANSAC算法与采集装置的投影误差尺度，求解匹配特征的Homography变换。RANSAC算法通过反复选择匹配特征中的一组随机特征描述子子集来达成目标。被选取的特征描述子子集被假设为局内点，然后：

1.利用作为局内点的特征集拟合一个适应于假设的局内点模型，所有的未知参数都能从假设的局内点计算得出。

2.用1中得到的模型去测试所有的特征描述子子集数据，如果某个子集适用于估计的模型，则将其也设为局内点。

3.如果被归类为假设的局内点数量超过预设限值（例如80%），那么1中模型就认定为合理。

4.用所有假设的局内点去重新估计1的模型。

5.最后，通过所有假设的局内点与模型的错误率来评估模型，例如错误率低于10%则最后的模型合理。

然后将上述匹配特征中除局内点外的点作为噪声去除，留下筛选后的匹配特征。此处的Homography变换是，是采集装置在两个位置进行了同一个参照对象2的图像采集，以Q点表示一个特征描述子相对于采集装置在第一位姿C处的坐标，Q’表示同个特征描述子相对于采集装置在第二位姿C’处的坐标，则Q和Q’之间的关系如式（2）所示：

重复带入匹配特征的特征描述子分别在第一位姿C处的坐标Q和第二位姿C’处的坐标Q’到式（2）中，获得一个采集装置的投影误差尺度最小的H值。H代表第一图像的任意一点和第二图像的相同点所形成的不同像素坐标之间的关系，即特征匹配的结果。

假设采集装置的内部参数矩阵为K，则q和q’分别为Q在C处和 Q’在C’处的像素坐标，其关系式如式（3）所示：

则任何成对像素坐标q和q’均满足如式（4）所示：

由式（4）可以推导出式（5）：

其中，d是相对距离，K采集装置的内部参数矩阵，n的T次方表示向量的转置，R和t分别是所述的采集装置在第一位姿C处和第二位姿C’处的相对旋转（rotation）和位移（translation）。

然后将式（2）计算最后获得的采集装置的投影误差尺度最小的H值代入式（5）进行分解得到R和t。

一些实施例中，步骤S03中，所述的优化所述Homography变换得到所述第一位姿与所述第二位姿的相对位姿关系采用线性优化，包括如下步骤：将所述Homography变换分解为四个可能解，然后利用平面法向量先验约束得到旋转向量和平移向量。具体是一组旋转矩阵R和平移向量t对应的像点P坐标深度不为负，将这组相机旋转矩阵R和平移向量t作为相机位姿变化的旋转矩阵R与平移向量t。此处的先验约束是：在安装配置下，平面法向量与采集装置坐标系Z轴的夹角为较小锐角。

一些实施例中，步骤S04中，所述的基于所述第一位姿与所述第二位姿的相对位姿关系，所述的相对距离和所述的相对角度计算，得到所述的第二位姿与所述的参照对象2的相对位姿关系，包括如下步骤：将所述的相对距离作用到所述第一位姿与所述第二位姿的相对位姿关系的平移向量中，从而得到真实平移距离。将所述的相对角度作用到所述第一位姿与所述第二位姿的相对位姿关系的旋转向量中，从而得到真实旋转角度。通过所述真实平移距离和真实旋转角度计算得到所述的第二位姿与所述的参照对象2的相对位姿关系。

一些实施例中，若所述采集装置采集第二参照对象2的第一图像后，基于所述第二参照对象2的第一图像提取特征得到的第一特征，与所述采集装置采集第一参照对象2的第一图像的第一特征相同，则获取所述的第一位姿与第二参照对象2所处平面的相对距离时，直接获取第一位姿与第一参照对象2所处平面的相对距离；以及获取所述第一位姿下第二采集装置的采集方向与所述参照对象2的相对角度时，直接获取所述第一位姿下第一采集装置的采集方向与所述参照对象2的相对角度。这是在两个参照对象2即使有所不同，但是在特征提取后获得的第一特征高度重合，就可以直接归类为同一种参照对象2，直接使用已有的参照对象2的数据，减少了计算和测量过程。

一些实施例中，若基于所述的第二图像提取特征，得到的第二特征由多组第二特征构成，则选取一组第二特征与所述的第一特征进行匹配计算。这是在采集装置采集到多个参照对象2的情况下的一个设置，一般会选择特征数量更多的第二特征，说明这个参照对象2与采集装置的位置更近，可以减少定位的误差。

在一个实施例中，如图2所示，提供了一种定位装置，用于设有采集装置的可移动平台1，包括：

获取模块110，用于获取第一位姿下采集装置采集参照对象2的第一图像，第二位姿下采集装置采集参照对象2的第二图像，第一位姿下采集装置与参照对象2所处平面的相对距离，第一位姿下采集装置的采集方向与所述参照对象2的相对角度。

提取模块120，用于基于所述的第一图像提取特征，得到第一特征；用于基于所述的第二图像提取特征，得到第二特征；

数据计算模块130，用于基于所述第一特征，所述第二特征，所述的第一位姿下采集装置与参照对象2所处平面的相对距离，所述的第一位姿下采集装置的采集方向与所述参照对象2的相对角度，计算获得所述的第二位姿与所述的参照对象2的相对位姿关系；

存储模块140，用于存储所述第一特征及其对应的参照对象2。

在其中一个实施例中，所述定位装置还包括位姿调节模块，用于检测所述采集装置当前位姿与第一位姿的偏差后，调整所述采集装置到第一位姿，所述的第一位姿为采集装置的采集方向垂直于所述参照对象2的位姿。

在其中一个实施例中，所述位姿调节模块包括：

测量数据获取单元，用于获取所述采集装置的第一测距点12和第二测距点13在当前位姿下，分别距离所述参照对象2的第一当前位姿距离和第二当前位姿距离。

偏移角度计算单元，用于基于所述第一当前位姿距离、所述第二当前位姿距离与第一测距点12和第二测距点13的间距计算实际偏移角度。

第一判断单元，用于判断所述实际偏移角度与预设偏移角度的大小，得到判断结果一；

第二判断单元，判断所述第一当前位姿距离和所述第二当前位姿距离的大小，得到判断结果二；

信号传输单元，基于所述判断结果一和所述判断结果二传输到可移动平台1。

在其中一个实施例中，所述数据计算模块130包括：

第一计算单元，用于通过Hamming距离对第一特征与第二特征进行关联匹配，得到匹配特征。

第二计算单元，用于结合RANSAC算法与采集装置的投影误差尺度，求解匹配特征的Homography变换。

优化单元，用于优化所述Homography变换得到所述第一位姿与所述第二位姿的相对位姿关系。

第三计算单元，基于所述第一位姿与所述第二位姿的相对位姿关系，所述的相对距离和所述的相对角度计算，得到所述的第二位姿与所述的参照对象2的相对位姿关系。

在其中一个实施例中，所述定位装置还包括选择模块，用于在基于所述的第二图像提取特征，得到的第二特征由多组第二特征构成时，选取一组第二特征发送到获取模块110。

所述的定位装置的定位方法与上述的基于参照对象2的定位方法相同，此处不再赘述。

在一个实施例中，提供了一种可移动平台1，包括采集装置，移动装置、移动控制装置、上述的各定位装置。所述采集装置与所述定位装置通信连接，所述移动控制装置与所述移动装置通信连接，所述定位装置与所述移动控制装置通信连接。采集装置采集第一图像、第二图像发送给定位装置，定位装置进行定位后，发送定位信息到移动控制装置，移动控制装置根据定位情况控制移动装置进行移动。

所述的可移动平台1的定位方法在上述各方法实施例中已经详细说明，此处不再赘述。

在一个实施例中，提供了一种计算机设备，该计算机设备可以是终端，其内部结构图可以如图3所示。该计算机设备包括通过系统总线连接的处理器、存储器、通信接口、显示屏和输入装置。其中，该计算机设备的处理器用于提供计算和控制能力。该计算机设备的存储器包括非易失性存储介质、内存储器。该非易失性存储介质存储有操作系统和计算机程序。该内存储器为非易失性存储介质中的操作系统和计算机程序的运行提供环境。该计算机设备的通信接口用于与外部的终端进行有线或无线方式的通信，无线方式可通过WIFI、运营商网络、NFC(近场通信)或其他技术实现。该计算机程序被处理器执行时以实现一种基于两种传感器数据融合的建图方法或一种基于两种传感器数据融合的定位方法。该计算机设备的显示屏可以是液晶显示屏或者电子墨水显示屏，该计算机设备的输入装置可以是显示屏上覆盖的触摸层，也可以是计算机设备外壳上设置的按键、轨迹球或触控板，还可以是外接的键盘、触控板或鼠标等。

本领域技术人员可以理解，图3中示出的结构，仅仅是与本申请方案相关的部分结构的框图，并不构成对本申请方案所应用于其上的计算机设备的限定，具体的计算机设备可以包括比图中所示更多或更少的部件，或者组合某些部件，或者具有不同的部件布置。

在一个实施例中，还提供了一种计算机设备，包括存储器和处理器，存储器中存储有计算机程序，该处理器执行计算机程序时实现上述各方法实施例中的步骤。具体的定位步骤上述各方法实施例中已经详细说明，此处不再赘述。

在一个实施例中，提供了一种计算机可读存储介质，其上存储有计算机程序，其上存储有计算机程序，该计算机程序被处理器执行时实现上述各方法实施例中的步骤。具体步骤上述各方法实施例中已经详细说明，此处不再赘述。

本领域普通技术人员可以理解实现上述实施例方法中的全部或部分流程，是可以通过计算机程序来指令相关的硬件来完成，所述的计算机程序可存储于一非易失性计算机可读取存储介质中，该计算机程序在执行时，可包括如上述各方法的实施例的流程。其中，本申请所提供的各实施例中所使用的对存储器、存储、数据库或其它介质的任何引用，均可包括非易失性和易失性存储器中的至少一种。非易失性存储器可包括只读存储器(Read-Only Memory，ROM)、磁带、软盘、闪存或光存储器等。易失性存储器可包括随机存取存储器(Random Access Memory，RAM)或外部高速缓冲存储器。作为说明而非局限，RAM可以是多种形式，比如静态随机存取存储器(Static Random Access Memory，SRAM)或动态随机存取存储器(Dynamic Random Access Memory，DRAM)等。

在本说明书的描述中，参考术语“一种实施方式”、“一些实施例”、“示例”、“具体示例”、或“一些示例”等的描述意指结合该实施例或示例描述的具体特征、结构、材料或者特点包含于本发明的至少一种实施方式或示例中。在本说明书中，对上述术语的示意性表述不一定指的是相同的实施例或示例。而且，描述的具体特征、结构、材料或者特点可以在任何的一个或多个实施例或示例中以合适的方式结合。

尽管上面已经示出和描述了本发明的实施例，但不能理解为是对权利要求的限制。本发明不仅限于以上实施例，其具体结构允许有变化，凡在本发明独立权利要求的保护范围内所作的各种变化均在本发明的保护范围内。

Claims (20)

1.一种基于参照对象的定位方法，用于设有采集装置的可移动平台，其特征在于：包括如下步骤：

获取第一图像，所述的第一图像基于采集装置位于第一位姿采集参照对象所得；基于所述的第一图像提取特征，得到第一特征；同时获取所述的第一位姿与参照对象所处平面的相对距离；以及获取所述第一位姿下采集装置的采集方向与所述参照对象的相对角度；

获取第二图像，所述的第二图像基于采集装置位于第二位姿采集参照对象所得；基于所述的第二图像提取特征，得到第二特征；

基于所述的第一特征和所述的第二特征进行匹配计算，得到所述第一位姿与所述第二位姿的相对位姿关系；

基于所述第一位姿与所述第二位姿的相对位姿关系，所述的相对距离和所述的相对角度计算，得到所述的第二位姿与所述的参照对象的相对位姿关系。

2.根据权利要求1所述的一种基于参照对象的定位方法，其特征在于，所述第一位姿为采集装置的采集方向垂直于所述参照对象的位姿。

3.根据权利要求2所述的一种基于参照对象的定位方法，其特征在于，所述的获取第一图像，所述的第一图像基于采集装置位于第一位姿采集参照对象所得之前，还包括如下步骤：检测所述采集装置当前位姿与第一位姿的偏差后，调整所述采集装置到第一位姿。

4.根据权利要求3所述的一种基于参照对象的定位方法，其特征在于，所述的检测所述采集装置当前位姿与第一位姿的偏差后，调整所述采集装置到第一位姿，具体包括如下步骤：

重复如下步骤，直至实际偏移角度处于预设阈值范围之内：

获得所述采集装置的第一测距点和第二测距点在当前位姿下，分别距离所述参照对象的第一当前位姿距离和第二当前位姿距离；通过所述第一当前位姿距离、所述第二当前位姿距离与第一测距点和第二测距点的间距计算实际偏移角度；判断所述实际偏移角度与预设偏移角度的大小，得到判断结果一；判断所述第一当前位姿距离和所述第二当前位姿距离的大小，得到判断结果二；基于所述判断结果一和所述判断结果二调整可移动平台。

5.根据权利要求4所述的一种基于参照对象的定位方法，其特征在于，所述的获取所述的第一位姿与参照对象所处平面的相对距离，包括如下步骤：获得所述采集装置的第一测距点和第二测距点在第一位姿下，分别距离所述参照对象的第一位姿距离和第二位姿距离，计算获得所述第一位姿距离和所述第二位姿距离的平均值为所述相对距离。

6.根据权利要求1所述的一种基于参照对象的定位方法，其特征在于，所述的基于所述的第一图像提取特征，得到第一特征，包括如下步骤：通过特征提取算法基于所述的第一图像提取特征点，并通过特征提取算法计算获得第一图像特征描述子，集合得到所述第一特征。

7.根据权利要求1所述的一种基于参照对象的定位方法，其特征在于，所述的基于所述的第二图像提取特征，得到第二特征，包括如下步骤：通过特征提取算法基于所述的第二图像提取特征点，并通过特征提取算法计算获得第二图像特征描述子，集合得到所述第二特征。

8.根据权利要求1所述的一种基于参照对象的定位方法，其特征在于，所述的基于所述的第一特征和所述的第二特征进行匹配计算，得到所述第一位姿与所述第二位姿的相对位姿关系，包括如下步骤：通过Hamming距离对第一特征与第二特征进行关联匹配，得到匹配特征；结合RANSAC算法与采集装置的投影误差尺度，求解匹配特征的Homography变换，优化所述Homography变换得到所述第一位姿与所述第二位姿的相对位姿关系。

9.根据权利要求8所述的一种基于参照对象的定位方法，其特征在于，所述的优化所述Homography变换得到所述第一位姿与所述第二位姿的相对位姿关系，包括如下步骤：将所述Homography变换分解为四个可能解，然后利用平面法向量先验约束得到旋转向量和平移向量。

10.根据权利要求9所述的一种基于参照对象的定位方法，其特征在于，所述的基于所述第一位姿与所述第二位姿的相对位姿关系，所述的相对距离和所述的相对角度计算，得到所述的第二位姿与所述的参照对象的相对位姿关系，包括如下步骤：将所述的相对距离作用到所述第一位姿与所述第二位姿的相对位姿关系的平移向量中，从而得到真实平移距离；将所述的相对角度作用到所述第一位姿与所述第二位姿的相对位姿关系的旋转向量中，从而得到真实旋转角度；通过所述真实平移距离和真实旋转角度计算得到所述的第二位姿与所述的参照对象的相对位姿关系。

11.根据权利要求1所述的一种基于参照对象的定位方法，其特征在于，若所述采集装置采集第二参照对象的第一图像后，基于所述第二参照对象的第一图像提取特征得到的第一特征，与所述采集装置采集第一参照对象的第一图像的第一特征相同，则获取所述的第一位姿与第二参照对象所处平面的相对距离时，直接获取第一位姿与第一参照对象所处平面的相对距离；以及获取所述第一位姿下第二采集装置的采集方向与所述参照对象的相对角度时，直接获取所述第一位姿下第一采集装置的采集方向与所述参照对象的相对角度。

12.根据权利要求1所述的一种基于参照对象的定位方法，其特征在于，若基于所述的第二图像提取特征，得到的第二特征由多组第二特征构成，则选取一组第二特征与所述的第一特征进行匹配计算。

13.一种定位装置，用于设有采集装置的可移动平台，其特征在于，包括：

获取模块，用于获取第一位姿下采集装置采集参照对象的第一图像，第二位姿下采集装置采集参照对象的第二图像，第一位姿下采集装置与参照对象所处平面的相对距离，第一位姿下采集装置的采集方向与所述参照对象的相对角度；

提取模块，用于基于所述的第一图像提取特征，得到第一特征；用于基于所述的第二图像提取特征，得到第二特征；

数据计算模块，用于基于所述第一特征，所述第二特征，所述的第一位姿下采集装置与参照对象所处平面的相对距离，所述的第一位姿下采集装置的采集方向与所述参照对象的相对角度，计算获得所述的第二位姿与所述的参照对象的相对位姿关系；

存储模块，用于存储所述第一特征及其对应的参照对象。

14.根据权利要求13所述的一种定位装置，其特征在于，所述定位装置还包括位姿调节模块，用于检测所述采集装置当前位姿与第一位姿的偏差后，调整所述采集装置到第一位姿，所述的第一位姿为采集装置的采集方向垂直于所述参照对象的位姿。

15.根据权利要求14所述的一种定位装置，其特征在于，所述位姿调节模块包括：

测量数据获取单元，用于获取所述采集装置的第一测距点和第二测距点在当前位姿下，分别距离所述参照对象的第一当前位姿距离和第二当前位姿距离；

偏移角度计算单元，用于基于所述第一当前位姿距离、所述第二当前位姿距离与第一测距点和第二测距点的间距计算实际偏移角度；

第一判断单元，用于判断所述实际偏移角度与预设偏移角度的大小，得到判断结果一；

第二判断单元，判断所述第一当前位姿距离和所述第二当前位姿距离的大小，得到判断结果二；

信号传输单元，基于所述判断结果一和所述判断结果二传输到可移动平台。

16.根据权利要求13所述的一种定位装置，其特征在于，所述数据计算模块包括：

第一计算单元，用于通过Hamming距离对第一特征与第二特征进行关联匹配，得到匹配特征；

第二计算单元，用于结合RANSAC算法与采集装置的投影误差尺度，求解匹配特征的Homography变换；

优化单元，用于优化所述Homography变换得到所述第一位姿与所述第二位姿的相对位姿关系；

第三计算单元，基于所述第一位姿与所述第二位姿的相对位姿关系，所述的相对距离和所述的相对角度计算，得到所述的第二位姿与所述的参照对象的相对位姿关系。

17.根据权利要求13所述的一种定位装置，其特征在于，所述定位装置还包括选择模块，用于在基于所述的第二图像提取特征，得到的第二特征由多组第二特征构成时，选取一组第二特征发送到获取模块。

18.一种可移动平台，其特征在于，包括采集装置，移动装置、移动控制装置、如权利要求13至17中任一项所述的定位装置；所述采集装置与所述定位装置通信连接，所述移动控制装置与所述移动装置通信连接，所述定位装置与所述移动控制装置通信连接。

19.一种计算机设备，包括存储器和处理器，所述存储器存储有计算机程序，其特征在于，所述处理器执行所述计算机程序时实现权利要求1至12中任一项所述基于参照对象的定位方法的步骤。

20.一种计算机可读存储介质，其上存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现权利要求1至12中任一项所述基于参照对象的定位方法的步骤。

Images