CN111415390A - 一种基于地面纹理的定位导航方法及装置 - Google Patents

Abstract

本发明提供了一种基于地面纹理的定位导航方法及装置。上述定位导航方法包括：获取当前帧的地面纹理图像；将上述当前帧的地面纹理图像与参考帧的地面纹理图像进行配准处理的第一配准，上述配准处理系对两张地面纹理图像频谱上的特征点所对应的离散频域至少通过离散相位相关法进行求解以得到两张地面纹理图像之间的相对位移与相对角度；以及响应于上述第一配准成功，基于上述参考帧的位置信息以及上述参考帧与上述当前帧之间的相对位移与相对角度确定上述当前帧的位置信息。本发明还提供了实现上述定位导航方法的定位导航装置。本发明所提供的定位导航方法及装置具有快速的地面纹理匹配计算速度，局部与全局定位精准，具有较高的稳定性与适应性。

Description

一种基于地面纹理的定位导航方法及装置

技术领域

本发明涉及定位导航领域，尤其涉及一种基于地面纹理图像识别的定位导航方法及装置。

背景技术

AGV小车(Automated Guided Vehicle)正在普及。AGV小车的核心技术主要为定位导航算法。主流的定位导航方法包括磁导航、二维码导航、激光导航和视觉导航。其中，基于地面纹理的视觉导航是一种新兴的导航方法，其适用性较为广泛，传感器成本低廉，易于部署。目前有采用点云匹配的地面纹理导航方法，还有采用相位相关法进行地面纹理匹配的地面纹理导航方法。

在当前的地面纹理导航方法中，现有技术CN101566471A公开了一种基于点云匹配的地面纹理导航方法，但该方法存在不稳定的问题。具体的，由于地面纹理随机性较高，很难定义出一个较为符合实际应用的地面纹理特征点提取方法。此外地面纹理多属于细小的纹理，基于点云匹配的算法可能会出现太多特征点，从而降低配准效率。

现有技术CN201710264333.9公开了一种基于相位相关法的地面纹理导航方法，但该方法存在计算速度较慢，计算单元成本较高的缺陷。具体的，由于二维离散傅里叶变换具有混异性，通过log-polar变换的二维离散傅里叶功率密度不是严格旋转不变的，因此需要对图像进行多次旋转变化并重新计算其二维傅里叶变换功率图，计算量庞大，计算速度较慢。此外由于输入的图片是实数域的，傅里叶变换后的功率密度是中心对称的，因此在计算旋转变化时会出现多解，需要排除其它错误角度，这也导致需多次进行二维傅里叶变换。由于二维傅里叶变化计算量较大，该方法必须使用高端计算设备或降低地面纹理导航的定位坐标输出速率。因此实际使用中要么产生了较高的成本，要么降低了设备运行性能。

并且，还存在以下若干问题并无法通过现有技术有效地解决：

(1)当小车由于故障等因素行驶到地图以外的区域，会导致地面纹理导航故障；

(2)当地面纹理细节动态范围较大时(如地面有一块显著的临时性油污，而纹理本身对比度较低)，基于点云匹配的算法只能找到显著特征点而会忽略较重要的低对比度的信息，从而可能解出错误的位移和旋转。基于傅里叶变换的算法会因显著区域的权重过大，倾向于得出无解结论；

(3)当地面出现重复性较高的纹理时，当前地面纹理导航方法会找到似是而非的位移和旋转；以及

(4)地面导航算法对视觉传感器校准精确度要求过于严格，在实际生产中由于震动、磨损等原因，难以确保仅仅出场校准后能够使用较长时间，因此需要经常对视觉传感器进行校准调试，增加后续维护成本。

因此，亟需要一种基于地面纹理的定位导航方法及装置，能够克服现有技术的点云匹配和相位相关法所存在的问题，能够一并解决现有技术无法解决的影响AGV小车精准、高效定位导航的问题，从而提高地面纹理的匹配速度、提高局部与全局的精准定位，使AGV小车的定位导航具有较高的稳定性与适应性。

发明内容

以下给出一个或多个方面的简要概述以提供对这些方面的基本理解。此概述不是所有构想到的方面的详尽综览，并且既非旨在指认出所有方面的关键性或决定性要素亦非试图界定任何或所有方面的范围。其唯一的目的是要以简化形式给出一个或多个方面的一些概念以为稍后给出的更加详细的描述之序。

为了解决上述问题，本发明提供了一种基于地面纹理的定位导航方法，包括：获取当前帧的地面纹理图像；

将上述当前帧的地面纹理图像与参考帧的地面纹理图像进行配准处理的第一配准，上述配准处理系对两张地面纹理图像频谱上的特征点所对应的离散频域至少通过离散相位相关法进行求解以得到两张地面纹理图像之间的相对位移与相对角度；以及

响应于上述第一配准成功，基于上述参考帧的位置信息以及上述参考帧与上述当前帧之间的相对位移与相对角度确定上述当前帧的位置信息。

在上述定位导航方法的一实施例中，可选的，上述定位导航方法还包括：

响应于上述参考帧与上述当前帧之间的相对位移超出预设阈值，更新上述参考帧的地面纹理图像为上述当前帧的地面纹理图像，更新上述参考帧的位置信息为上述当前帧的位置信息。

在上述定位导航方法的一实施例中，可选的，上述地面纹理图像为边长为w的正方形；上述超出预设阈值进一步包括：

响应于上述参考帧与上述当前帧之间x方向或y方向的相对位移大于0.5w，则认为上述参考帧与上述当前帧之间的相对位移超出预设阈值。

在上述定位导航方法的一实施例中，可选的，上述定位导航方法还包括：

响应于上述第一配准失败，基于上述参考帧与上一帧之间的相对位移与相对角度确定上述参考帧与上述当前帧之间的预估相对位移与预估相对角度；

基于上述参考帧的位置信息以及上述参考帧与上述当前帧之间的预估相对位移与预估相对角度确定上述当前帧的位置信息；以及

更新上述参考帧的地面纹理图像为上述当前帧的地面纹理图像，更新上述参考帧的位置信息为上述当前帧的位置信息。

在上述定位导航方法的一实施例中，可选的，基于上述参考帧与上一帧之间的相对位移与相对角度确定上述参考帧与上述当前帧之间的预估相对位移与预估相对角度进一步包括：

获取上述当前帧的速度；

获取上述上一帧与上述当前帧之间的时间差；以及

基于上述速度、上述时间差以及上述参考帧与上一帧之间的相对位移与相对角度确定上述参考帧与上述当前帧之间的预估相对位移与预估相对角度。

在上述定位导航方法的一实施例中，可选的，响应于上述参考帧的位置信息被更新为上述当前帧的位置信息，上述定位导航方法还包括：

将上述参考帧更新后的地面纹理图像与预设的若干地图帧的地面纹理图像逐一进行上述配准处理的若干第二配准；以及

响应于任意第二配准成功，根据该配准成功的地图帧的位置信息以及该配准成功的地图帧与上述参考帧之间的配准相对位移与配准相对角度校准上述参考帧的位置信息。

在上述定位导航方法的一实施例中，可选的，响应于任意第二配准成功，上述定位导航方法还包括：

基于上述参考帧校准前的位置信息与该配准成功的地图帧的位置信息获取该配准成功的地图帧与上述参考帧之间的预估相对位移和预估相对角度；

获取上述当前帧的上一帧与更新前的参考帧之间的配准相对位移；

基于该配准成功的地图帧与上述参考帧之间的配准相对角度、预估相对角度以及上述当前帧的上一帧与更新前的参考帧之间的配准相对位移对上述地面纹理图像的摄像装置进行俯仰校正。

在上述定位导航方法的一实施例中，可选的，上述配准处理进一步包括：

至少通过二维快速傅立叶变换得到两张地面纹理图像频谱上去混叠化的特征点点集P1f、P2f；

基于特征点点集P1f、P2f提取两张地面纹理图像之间的相对角度th，上述相对角度th使得特征点点集P1f与旋转了th度后的特征点点集P2f之间有最多的重合点对PL(i)；以及

获取每个重合点对PL(i)分别在两张地面纹理图像上的相位差ph(i)，以通过离散相位相关法得到相关平面图R，其中

响应于上述相关平面图R中峰值点的峰值量小于预设常数，则配准失败；

响应于上述峰值点的峰值量大于等于上述预设常数，基于上述峰值点在上述相关平面图R中的坐标(x,y)获取上述两张地面纹理图像之间的相对位移。

在上述定位导航方法的一实施例中，可选的，得到上述特征点点集P1f、P2f进一步包括：

分别对待处理的两张地面纹理图像进行全频域的二维快速傅立叶变换并取模得到两张地面纹理图像的谱功率A1、A2；

提取上述谱功率A1、A2的特征点中特征响应幅值最高的若干点组成特征点点集P1、P2，并记录点集P1、P2中各点的功率；

将上述特征点点集P1、P2旋转c度以得到特征点点集P1c、P2c，使用离散傅立叶变换基于旋转了c度后的两张地面纹理图像获取特征点点集P1c、P2c中各点的功率，其中，c度在3-5度之间；以及

取特征点点集P1和P1c中功率差异最小的若干点组成特征点点集P1f，取特征点点集P2和P2c中功率差异最小的若干点组成特征点点集P2f。

在上述定位导航方法的一实施例中，可选的，提取上述相对角度th进一步包括：

多次随机选择特征点点集P1f和P2f中的点P1f(i)和P2f(j)，计算P1f(i)和P2f(j)至原点的距离是否接近；

响应于接近，基于点P1f(i)和P2f(j)的坐标计算两张地面纹理图像之间的假定相对角度th’，计算特征点点集P1f与旋转了th’度后的特征点点集P2f之间的重合点对PL(i)的数量；以及

在多个假定相对角度th’中选择使得特征点点集P1f与旋转了th’度后的特征点点集P2f之间的重合点对PL(i)的数量最大的假定相对角度th’为上述相对角度th。

在上述定位导航方法的一实施例中，可选的，响应于存在两张地面纹理图像的预估相对角度thp，提取上述相对角度th还包括：

在计算得到假定相对角度th’后，判断该假定相对角度th’与上述预估相对角度thp是否接近，响应于接近，计算特征点点集P1f与旋转了th’度后的特征点点集P2f之间的重合点对PL(i)的数量。

在上述定位导航方法的一实施例中，可选的，响应于存在两张地面纹理图像的预估相对位移(xp,yp)，基于上述峰值点在上述相关平面图R中的坐标获取上述两张地面纹理图像之间的相对位移进一步包括：

分别计算位移(x,y)、(x-w,y)、(x,y-w)、(x-w,y-w)和上述预估相对位移(xp,yp)之间的差异，取位移(x,y)、(x-w,y)、(x,y-w)、(x-w,y-w)中与上述预估相对位移(xp,yp)之间差异最小的位移为上述两张地面纹理图像之间的相对位移；其中

w为上述地面纹理图像的边长。

在上述定位导航方法的一实施例中，可选的，响应于不存在两张地面纹理图像的预估相对位移，基于上述峰值点在上述相关平面图R中的坐标获取上述两张地面纹理图像之间的相对位移进一步包括：

分别测试两张地面纹理图像的其中之一在旋转了th度、位移了(x,y)、(x-w,y)、(x,y-w)、(x-w,y-w)后，两张地面纹理图像之间的重叠度，取位移(x,y)、(x-w,y)、(x,y-w)、(x-w,y-w)后重叠度最高的位移为上述两张地面纹理图像之间的相对位移；其中

w为上述地面纹理图像的边长。

在上述定位导航方法的一实施例中，可选的，上述定位导航方法还包括：对地面纹理图像进行预处理，以将原始图像I转化为定义在复数域上的边缘方向均衡化的结果图像Y；其中

上述配准处理的两张地面纹理图像系经过预处理后的两张结果图像。

在上述定位导航方法的一实施例中，可选的，上述预处理进一步包括：

获取上述原始图像I在x、y方向的梯度图Ix、Iy；

基于上述梯度图Ix、Iy获取上述原始图像I的边缘图E；

基于上述梯度图Ix、Iy和上述边缘图E获取边缘方向图D；

基于上述边缘图E中的N个随机像素获取各个像素值的频数表H，对上述频数表H中的每一项做累加和以得到累加频数表H’；以及

基于上述频数表H、上述累加频数表H’均衡化上述边缘方向图D以得到上述结果图像Y。

在上述定位导航方法的一实施例中，可选的，上述结果图像Y中像素x,y的像素值Y(x,y)＝(H(E(x,y))/H’(255))^F*D(x,y)；其中

E(x,y)＝Ix(x,y)*Ix(x,y)+Iy(x,y)*Iy(x,y)；

D(x,y)＝(Ix(x,y)+i*Iy(x,y))/E(x,y)，i为复数虚部；以及

F为预设常数。

本发明还提供了一种基于地面纹理的定位导航装置，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，其中，上述处理器执行上述计算机程序时实现如上述基于地面纹理的定位导航方法任一实施例中的步骤。

本发明还提供了一种AGV小车，上述AGV小车包含如上述的基于地面纹理的定位导航装置，以用于上述AGV小车的定位导航。

本发明还提供了一种计算机可读存储介质，其上存储有计算机程序，其特征在于，上述计算机程序被处理器执行时实现如上述基于地面纹理的定位导航方法任一实施例中的定位导航方法的步骤。

根据本发明所提供的基于地面纹理的定位导航方法及装置的一方面，通过匹配当前帧与参考帧，保证任何时刻都有坐标输出，甚至是在无地图的场景下也能够输出短时间内准确的坐标，保证了局部定位的准确性。

根据本发明所提供的基于地面纹理的定位导航方法及装置的一方面，通过使用更快捷的计算方法提高地面纹理匹配的计算速度、保证了计算结果的精确性与稳定性，降低了计算量与计算时间，从而降低计算单元的成本，可以兼容地部署在较低端的计算设备上。

根据本发明所提供的基于地面纹理的定位导航方法及装置的一方面，还可以通过独立的线程对参考帧进行位置校准，从而保证全局定位的准确性，并且双线程独立同时运行，可以充分利用当前流行的多核计算设备平台提高计算速度和计算精确度。

根据本发明所提供的基于地面纹理的定位导航方法及装置的一方面，通过对的地面纹理导航结果进行统计，能够分析得到当前的摄像头姿态，从而具有一定程度的摄像头俯仰参数的自动校准能力，能够有效地减少由于震动、轮子磨损等引发的摄像头相对地面姿态变化而必须由人工校准摄像头的次数，降低了后续维护成本。

附图说明

在结合以下附图阅读本公开的实施例的详细描述之后，能够更好地理解本发明的上述特征和优点。在附图中，各组件不一定是按比例绘制，并且具有类似的相关特性或特征的组件可能具有相同或相近的附图标记。

图1示出了根据本发明所提供的定位导航方法一方面的流程图。

图2示出了根据本发明所提供的定位导航装置一方面的示意图。

图3示出了根据本发明所提供的AGV小车一方面的示意图。

附图标记

200 定位导航装置

210 处理器

220 存储器

300 AGV小车

310 摄像装置

具体实施方式

以下结合附图和具体实施例对本发明作详细描述。注意，以下结合附图和具体实施例描述的诸方面仅是示例性的，而不应被理解为对本发明的保护范围进行任何限制。

给出以下描述以使得本领域技术人员能够实施和使用本发明并将其结合到具体应用背景中。各种变型、以及在不同应用中的各种使用对于本领域技术人员将是容易显见的，并且本文定义的一般性原理可适用于较宽范围的实施例。由此，本发明并不限于本文中给出的实施例，而是应被授予与本文中公开的原理和新颖性特征相一致的最广义的范围。

在以下详细描述中，阐述了许多特定细节以提供对本发明的更透彻理解。然而，对于本领域技术人员显而易见的是，本发明的实践可不必局限于这些具体细节。换言之，公知的结构和器件以框图形式示出而没有详细显示，以避免模糊本发明。

请读者注意与本说明书同时提交的且对公众查阅本说明书开放的所有文件及文献，且所有这样的文件及文献的内容以参考方式并入本文。除非另有直接说明，否则本说明书(包含任何所附权利要求、摘要和附图)中所揭示的所有特征皆可由用于达到相同、等效或类似目的的可替代特征来替换。因此，除非另有明确说明，否则所公开的每一个特征仅是一组等效或类似特征的一个示例。

注意，在使用到的情况下，标志左、右、前、后、顶、底、正、反、顺时针和逆时针仅仅是出于方便的目的所使用的，而并不暗示任何具体的固定方向。事实上，它们被用于反映对象的各个部分之间的相对位置和/或方向。此外，术语“第一”、“第二”仅用于描述目的，而不能理解为指示或暗示相对重要性。

在本发明的描述中，需要说明的是，除非另有明确的规定和限定，术语“安装”、“相连”、“连接”应做广义理解，例如，可以是固定连接，也可以是可拆卸连接，或一体地连接；可以是机械连接，也可以是电连接；可以是直接相连，也可以通过中间媒介间接相连，可以是两个元件内部的连通。对于本领域的普通技术人员而言，可以具体情况理解上述术语在本发明中的具体含义。

注意，在使用到的情况下，进一步地、较优地、更进一步地和更优地是在前述实施例基础上进行另一实施例阐述的简单起头，该进一步地、较优地、更进一步地或更优地后带的内容与前述实施例的结合作为另一实施例的完整构成。在同一实施例后带的若干个进一步地、较优地、更进一步地或更优地设置之间可任意组合的组成又一实施例。

为了解决现有技术中AGV小车所存在的各种问题，本发明提供了一种基于地面纹理的定位导航方法，以应用于AGV小车解决上述存在的各种问题。请参考图1来理解本发明所提供的定位导航方法一方面的具体实施方法。

如图1所示出的，本发明所提供的导航方法可以包括视觉里程计与地图匹配两大部分。其中，视觉里程计用以基于参考帧的地面纹理图像来配准当前帧的地面纹理图像，从而根据参考帧与当前帧之间的相对位移与相对角度确定当前帧的位置，从而能够用来确定AGV小车的位置和姿态。通过视觉里程计，能够知晓AGV小车前进的距离以及前进的方向。

地图匹配部分用以将视觉里程计中的参考帧与预设的若干地图帧进行配准，从而能够使得参考帧在预设的地图中被定位，即能够从全局的角度来确定AGV小车的位置和姿态。也就是说，视觉里程计不仅可以在没有地图的时候通过局部区域的定位来进行AGV小车的定位，更可以在参考帧被地图匹配后消除视觉里程计中的累计误差，准确从全局定位AGV小车。

可以理解的是，本发明所提供的用于AGV小车的定位导航方法系基于地面纹理图像，因此，需要对一系列的地面纹理图像进行处理。为了得到AGV小车的当前位置，需要通过摄像装置获取当前帧的地面纹理图像。

为了处理的方便，在本发明中，地面纹理图像均被预处理为边长为w的正方形。同时，为了进一步提高地面纹理导航的稳定性与适应性，在本发明所提供的定位导航方法中，对地面纹理图像的预处理还包括将将原始图像I转化为定义在复数域上的边缘方向均衡化的结果图像Y。可以理解的是，在本发明所提供的方法中，后续用来配准的地面纹理图像均是经过预处理后的结果图像。

具体的，本发明所提供的关于预处理的一方面中，地面纹理图像预处理的输入为一张包含地面纹理的照片I，通过以下步骤输出预处理结果Y。

1、对原始地面纹理照片I求x和y方向梯度图，记为Ix和Iy。

2、求出地面纹理照片的边缘图，记为E；并求出边缘方向图，记为D。

E(x,y)＝Ix(x,y)*Ix(x,y)+Iy(x,y)*Iy(x,y)。

D(x,y)＝(Ix(x,y)+i*Iy(x,y))/E(x,y)

其中E(i,j)表示图E中坐标为x,y的像素的值，Ix(x,y)、Iy(x,y)以此类推，i为复数虚部。

3、从图E中随机挑选N个像素，并统计各个像素值出现的频数，得到列表H。其中，N是个预先选定的参数，在一实施例中可以选择N为4096；可以理解的是，在一般计算机处理的图像中，像素值分布为0到255。

4、对H的每项做累加和，得到H’。

5、对图E中每个像素E(x,y)做处理得到结果图Y。

Y(x,y)＝(H(E(x,y))/H’(255))^F*D(x,y)，其中F是个人工指定的常数，在一实施例中，F可以取4。该步骤5用于使图像Y直方图均衡化，参数F用以控制均衡曲线，参数越大，则认为过曝区域越小。

本发明的一方面通过将原始输入的图片转化为一个复数域上定义的边缘经过了平均化的图，保留了原始图上主要的信息并且可直接在频域中进行处理，该图经过傅里叶变化处理后不具有中心对称的不良特点，对于纹理定位应用更易于确定角度。

为了得到AGV小车的位置信息，本发明的一方面视觉里程计中对当前帧的地面纹理图像和参考帧的地面纹理图像进行第一配准，以获得当前帧与参考帧之间的相对位移与相对角度，从而能够根据参考帧的位置信息知晓当前帧的位置信息。为了得到参考帧在全局中的准确位置，本发明的另一方面地图匹配中对参考帧的地面纹理图像与若干预设的地图帧进行第二配准，以获得参考帧与配准成功的地图帧之间的相对位移与相对角度，从而能够根据在全局中已经有准确位置信息的地图帧知晓参考帧在全局中的准确位置信息。

因此，根据本发明的一方面，以下将详细描述如何对两张地面纹理图像进行配准以获得两张地面纹理图像之间的相对位移以及相对角度。地面纹理图像的配准处理的输入为经过预处理后的两张包含地面纹理的照片Y1、Y2。通过以下步骤得到照片Y1和Y2间的相对位移x,y和相对角度th。配准的输入还可额外选择设定预估相对位移xp,yp和预估相对角度thp。

具体的，本发明所提供的配准可以进一步包括：至少通过二维快速傅立叶变换得到两张地面纹理图像频谱上去混叠化的特征点点集P1f、P2f；

基于特征点点集P1f、P2f提取两张地面纹理图像之间的相对角度th，相对角度th使得特征点点集P1f与旋转了th度后的特征点点集P2f之间有最多的重合点对PL(i)；以及

获取每个重合点对PL(i)分别在两张地面纹理图像上的相位差ph(i)，以通过离散相位相关法得到相关平面图R，其中响应于相关平面图R中峰值点的峰值量小于预设常数，则配准失败；响应于峰值点的峰值量大于等于预设常数，基于峰值点在相关平面图R中的坐标(x,y)获取两张地面纹理图像之间的相对位移。

至少通过二维快速傅立叶变换得到两张地面纹理图像频谱上去混叠化的特征点点集P1f、P2f包括：

1、对Y1、Y2进行全频域的二维快速傅里叶变换，得到F1、F2分别为Y1、Y2的谱。

2、对F1、F2取模，得到A1、A2分别为Y1、Y2的谱功率。

3、对A1、A2求特征点。本领域技术人员可以通过诸如Harris角点、单峰点或通过深度神经网络提取的特征点等方法提取特征点，并不为限。

4、对步骤3中得到的A1、A2特征点中，取特征响应幅值最高的前M个点，得到点集P1、P2，并记录点上功率和相位备用。其中M是预先设定的常数，例如取256。本领域技术人员可以由xy方向梯度差、图像的高斯差、深度神经网络的响应输出等方法提取特征响应幅值，并不为限。

5、将Y1、Y2旋转c度，得到图Y1c和Y2c。其中c是一个较小角度，一般为3到5度，用于处理二维离散傅里叶变换中的混叠现象。

6、对点集P1、P2旋转c度，得到P1c、P2c。

7、使用离散傅里叶变换算法(例如Goerztel算法，需要注意的是，离散傅立叶变换算法的不同实现方式不应不当地限定本发明的保护范围)，在图Y1c和Y2c中计算P1c、P2c位置所对应频率上的功率，在各图中取与步骤4中得到的点集P1、P2对应的功率差异最小的N个特征点，得到点集P1f、P2f。其中N是一个预先指定的常数，例如取128。

在得到特征点点集P1f、P2f后，基于特征点点集P1f、P2f提取两张地面纹理图像之间的相对角度th，相对角度th使得特征点点集P1f与旋转了th度后的特征点点集P2f之间有最多的重合点对PL(i)。具体的，在一实施中，本发明采用RANSAC方法提取Y1和Y2间角度th。具体的步骤如下：

1、多次随机选择P1f的一点P1f(i)，和P2f上的一点P2f(j)，计算其距离原点位置是否接近，其中，i和j分别为随机选择的编号。原点坐标是(0,0)，假设随机选择的P1f(i)＝(10,20)，P2f(j)＝(21,10)，那么他们和原点距离分别是22.4和23.4；判断接近可以同过预设一个常数(例如1)，比如距离的差值小于该常数的都认为是接近。

2、如果不接近，则重新选择P1f(i)、P2f(j)。

3、如果接近，则先假定Y1和Y2的假定相对角度为：

th’＝atan2(P1f(i).y,P1f(i).x)-atan2(P2f(j).y,P2f(j).x)。

其中atan2为反正切函数，P1f(i).x表示P1f点集中第i个点的x坐标，P1f(i).y等记号同理。

4、在得到假定相对角度th’后，若在配准的输入端一并设置了预估相对角度thp，则还需要比较假定相对角度th’与预估相对角度thp是否接近，若接近才继续，若不接近，则仍然需要重新随机选择P1f(i)、P2f(j)。通过设置预估相对角度，能够进一步筛选出更为何时的假定相对角度，从而不仅能够减少后续的计算工作量，还能使得所确定的相对角度更为精确。需要注意的是，在不同的配准过程中所提前设定的预估相对角度的计算方法各有不同，请一并结合具体的视觉里程计过程和地图匹配过程来理解预估相对角度，在此不再赘述。

5、计算点集P1f和旋转了th’度后的点集P2f之间重合的点的对数，记为C(i,j)，并对应地记录所旋转的角度th’{i,j}备用。

6、在众多的th’{i,j}中取使C(i,j)最大的(i,j)对，输出两张地面纹理图像之间的相对角度th为该(i,j)对对应的th’{i,j}，，并记其重合的点编号对列表PL备用。

也就是说，假设存在点P1f(100)、P2f(200)，这两点分别到原点的距离是接近的，那么根据点P1f(100)、P2f(200)假设Y1、Y2的相对角度为th’(100,200)。点集P1f和旋转了th’(100,200)度之后的点集P2f之间重合的点的对数为100，即记为C(100,200)＝100，假设由于在众多C(i,j)中，C(100,200)＝100是其中的最大值，则认为这两张图的重合度最大，因此，选择C(100,200)对应的角度th’(100,200)为两张图之间的相对角度th。

在得到重合的点编号对列表PL后，根据本发明的一方面，基于离散相位相关法求解得到两张地面纹理图像之间的相对位移。具体的：

1、对PL中所有的编号对PL(i)＝(m,n)，取P1f(m)和P2f(n)对应A1和A2上的相位，并作差得到ph(i)。其中i为选择的编号，PL(i)＝(m,n)表示PL列表的第i对是个重合点对，其中，点集P1f中第m个点和旋转了th’度的点集P2f中第n个点是重合的。

2、对相位差ph(i)，应用二维频率P1f(PL(i).m)，做逆离散傅里叶变换并取绝对值，得到相关平面图R。

3、取相关平面图R的峰值点坐标(x,y)，其中，需要判断该峰值点的值是否大于等于以预设常数P，响应于大于预设常数P，则认为配准成功，继续计算相对位移，否则则认为配准失败。

计算相对位移还需要进一步判断在配准的输入端是否一并设置了预估相对位移xp,yp，若预设了预估相对位移，则执行步骤4.1，否则执行步骤4.2，具体的：

4.1、存在预设的预估相对位移(xp,yp)，则分别测试位移(x,y)、(x-w,y)、(x,y-w)、(x-w,y-w)和预估相对位移(xp,yp)之间的差异，其中，w系地面纹理图像的边长。取位移(x,y)、(x-w,y)、(x,y-w)、(x-w,y-w)中与预估相对位移(xp,yp)差异最小的位移作为两张地面纹理图像之间的相对位移输出。

4.2、不存在预设的预估相对位移(xp,yp)，取地面纹理图像的边长为w，分别测试Y1和位移了(x,y)、(x-w,y)、(x,y-w)、(x-w,y-w)，旋转了th度的Y2’之间的重叠度。取重叠度最高的Y2’的位移量作为两张地面纹理图像之间的相对位移输出。可以理解的是，上述的重叠度可以是图片像素值绝对值差异、信息熵等，本领域技术人员可以根据现有的或将有的计算两张图片之间重叠度的方式来确定两张地面纹理图像之间的重叠度，计算重叠度的方法不应不当的限制本发明的保护范围。

本发明的一方面所提供的关于两张图像的配准通过在频域中求特征点的方法，由于频域中的特征主要以孤立斑点出现，因此特征点较为显著稳定。同时具有点云匹配方法计算速度快的属性，和频域变换方法计算结果精确稳定的属性。相对于现有技术中的点云匹配方法而言，大幅度提高了特征点的稳定性，且本发明的点云匹配只在旋转这一个参数上进行，大幅度减少了搜索空间，从而提高了稳定性。

本发明的一方面所提供的关于两张图像的配准只对图像Y1、Y2均只做了一次全频域的傅里叶变换变换，两张图像之间的相对位移通过离散相位相关方法求得，相对于现有技术中的相位相关法而言，无需多次对图像旋转并求傅里叶变换的步骤，因而能够大大减少了傅里叶变换的次数，从而节省了计算时间，进而可部署在较低端的计算设备上。

在描述了本发明的一方面关于如何进行两张图像的配准后，以下将具体描述在本发明的一方面关于视觉里程计中的如何利用两张地面纹理图像之间的配准来获得AGV小车当前的位置和姿态信息。

具体的，在初始化时，预设参考帧的地面纹理图像为初始化时所捕捉的地面纹理图像，参考帧的位置信息(位姿)为相较于预设指定的明确全局位置的地图帧的初始化量(Rx,Ry,Rth)。此处的三元数(Rx,Ry,Rth)中的三个分量分别表征该参考帧与指定的地图帧之间的x方向上的相对位移、y方向上的相对位移以及相对角度。在初始化时，还需要初始化速度V为(0,0,0)，这里的三元数(0,0,0)中的三个分量分别代表x方向上的位移速度、y方向上的位移速度以及旋转速度。

在本发明所提供的视觉里程计中，还包括设置“上一帧”的概念，所谓“上一帧”是指在当前帧D之前与参考帧配准成功的时间上最接近的那一帧。在初始化时，还需要初始化上一帧P的位姿为(0,0,0)，此处三元数(0,0,0)表征上一帧P与参考帧之间的x方向上的相对位移、y方向上的相对位移以及相对角度。另外，还需要初始化上一帧的捕捉时间为初始化时捕捉参考帧的地面纹理图像的捕捉时间，即在初始化时将参考帧认定为上一帧。

在视觉里程计中，通过循环地捕捉摄像头图片来进行地面纹理导航：

1、捕捉一帧当前摄像头的纹理画面为当前帧的地面纹理图像并记录该捕获时间。

2、计算上一帧的捕获时间和当前帧的捕获时间之间的时间差t。

3、通过速度V、时间差t以及上一帧P的位姿(Px,Py,Pth)来预估当前帧与参考帧之间的预估位姿P’＝P+V*t，即预估位姿P’为(Px’,Py’,Pth’)，(Px’,Py’)为预估相对位移，Pth’为预估相对角度。

4、以当前帧的地面纹理图像和参考帧的地面纹理图像以及预估位姿作为配准处理的输出，通过本发明的一方面所提供的配准处理的方法进行配准。

5、若配准成功，则记录返回的配准结果，即当前帧与参考帧之间的相对位移与相对角度，记录为K＝(dx,dy,dth)；

5.1、更新速度为V＝(K-P)/t

5.2、更新上一帧P的位姿为K

5.3、独占参考帧(地图匹配线程可能会修改参考帧的位置信息，此处是为了放置地图匹配线程与视觉里程计线程同时处理参考帧，造成冲突)，随后确定当前帧位置与坐标(即当前帧在全局中的位置)为：

x＝Rx+cos(Rth)*dx-sin(Rth)*dy

y＝Ry+sin(Rth)*dx+cos(Rth)*dy

th＝Rth+dth。

6、若配准失败，则独占参考帧，仍然使用下述的公式确定当前帧位置与坐标(即当前帧在全局中的位置)为：

x＝Rx+cos(Rth)*dx-sin(Rth)*dy

y＝Ry+sin(Rth)*dx+cos(Rth)*dy

th＝Rth+dth。

只是，响应于配准失败，上述公式中的dx为预估位姿P’中的预估相对位移Px’，dy为预估相对位移Py’，dth为来预估相对角度Pth’，也就是通过预估位姿P’来估计当前帧的位置。从而能够保证在视觉里程计线程中，始终能够有当前帧的位姿信息输出，从而能够知道当前这的局部或全局定位以用以导航。

7、当存在配准失败的情况时，或者，存在当前帧配准后的相对位移x大于图像尺寸w的一半或者相对位移y大于图像尺寸w的一半时，为了消除视觉里程计的累计偏差，需要重置参考帧，即

7.1、更新参考帧的地面纹理图像为当前帧的地面纹理图像，更新参考帧的位置信息为步骤5或6中确定的当前帧的位置信息，即：

Rx＝x,Ry＝y,Rth＝th。

7.2、通知地图匹配线程对参考帧进行对比，并附加参考帧的上一帧P的位姿数据用于摄像装置的自动校准。

7.3、重置上一帧P的位姿为(0,0,0)。

根据本发明的一方面所提供的用以确定当前帧位置信息的视觉里程计中，当前帧n的配准对象是参考帧，而非现有常见的n-1，由于每一次配准时所确定的位姿的测量并非绝对精确的，也就是说，即便小车停在一个地方不动，测出来的位姿变化也不为零，而是个接近于0的值，确切地说，这个和的分布是“二维随机行走”的分布，几乎不可能为0。但通过调整当前帧n的配准对象为一段时间(或距离)内不变的参考帧，即可以解决上述出现零漂，定位不准确的问题。

如上所述的，为了得到当前帧在全局中的准确定位，需要确定参考帧在全局中的定位。但是在上述视觉里程计线程中更新后的参考帧的位置信息是通过视觉里程计配准或者根据上一帧预估得到的，可能存在参考帧全局位置的偏差，为了修正参考帧在全局中的定位，本发明的一方面还提供了地图匹配线程。

具体的，在地图匹配线程的初始化中，需要向地图匹配线程预先输出地图数据，上述的数据为各个确定的位置上的地面纹理图像。

随后响应于接收到视觉里程计线程发出的参考帧配准通知，首先确定待校准的参考帧的位置为x1,y1,th1，根据参考帧的位置信息选择若干个距离该参考帧位置最近的地图帧，依据距离从小到大的顺序进行待校准的参考帧与地图帧之间的配准。

进一步的，在配准处理的输入端，可以预先确定待校准的参考帧与当前进行配准的地图帧之间的预估相对位移与角度。记待校准的参考帧位置为xr,yr,thr；当前进行配准的地图帧位置为xt,yt,tht，则预估相对位移和预估相对角度为：

X方向位移dxt’：(xt-xr)*cos(tht)+(yt-yr)*sin(tht)

Y方向位移dyt’：(xr-xt)*sin(tht)+(yt-yr)*cos(tht)

预估相对角度dtht’：tht-thr

随后通过上述的本发明的一方面所提供的配准方法对待校准的参考帧与地图帧依次按照距离，由近及远的进行配准。

一旦出现能够与带校准的参考帧成功配准的地图帧，独占参考帧，获取该当前进行配准的地图帧的位置xt,yt,tht，获取配准成功得到的相对位姿T(dxt,dyt,dtht)，将配准结果T(dxt,dyt,dtht)应用于地图帧位置(xt,yt,tht)上，从而得到待校准的参考帧位置并予以更新。

由于地图帧是按照与带校准的参考帧之间的距离依次与参考帧进行配准的，因此，响应于出现成功配准的地图帧，可以认为能够对参考帧进行位置校准。

根据地图匹配线程，能够有效地校准在视觉里程计线程中的参考帧在全局中的位置信息，从而能够消除纯粹的视觉里程计线程导致的相对位移之间的累计误差，能够避免出现定位不准的情况。同时，可以设定地图匹配线程与视觉里程计线程为两个独立的线程，从而能够提高地面纹理图像的处理速度，提高定位效率。

根据本发明的另一方面，还提供了用以对摄像装置的自动校准步骤。摄像头校准模型为：一个四棱锥和一个平面交叉，取平面和四棱锥的交叉面作摄像头切割画面的形状。随后将该形状拉伸为正方形，作纹理导航的实际使用画面。摄像头俯仰自动校准为自动调节平面的俯仰角度，使得摄像头切割画面的形状对应地面的形状正好是正方形。

具体的步骤如下：

1.等待俯仰校准数据：俯仰方向和修正量

2.将平面在俯仰方向上进行少量正比于修正量的微调。

其中，上述的俯仰方向和修正量需要通过视觉里程计配准结果的中的上一帧位姿(可以理解为当前帧的上一帧的“当前帧位姿”，因此，图1中用虚线表征)和地图匹配线程中的位姿数据来确定的。

具体的，在视觉里程计部分的步骤7.2中已经提及的了上一帧位姿为px,py,pth。在地图匹配线程，取预估相对角度dtht’：tht-thr以及实际配准所得的相对角度dtht，设定摄像头俯仰方向为(py,-px)，修正量为(dtht-dtht’)，以此来对摄像转置进行自动校准。

也就是说，对于(py，-px)这个方向(如果转换成角度的话，是atan2(py，-px))，假设摄像转置安装在云台三脚架上，自动校准俯仰为：对摄像装置向(py，-px)用dtht-dtht’的力推一下。从而能够能够有效地减少由于震动、轮子磨损等引发的摄像头相对地面姿态变化而必须由人工校准摄像头的次数。

综上，已经描述了本发明提供的基于地面纹理的定位导航方法的各个方面的具体实现方式。根据本发明的一方面，通过将原始输入的图片转化为一个复数域上定义的边缘经过了平均化的图，保留了原始图上主要的信息且可直接在频域中进行处理，该图经过傅里叶变化处理后不具有中心对称的不良特点，对于纹理定位应用更易于确定角度。

根据本发明的一方面，通过在频域中求特征点的方法，由于频域中的特征主要以孤立斑点出现，因此特征点较为显著稳定。同时具有点云匹配方法计算速度快的属性，和频域变换方法计算结果精确稳定的属性。相对于现有技术中的点云匹配方法而言，能够大幅度提高了特征点的稳定性，且本发明中的点云匹配只在旋转这一个参数上进行，大幅度减少了搜索空间，从而提高了稳定性。

根据本发明的一方面，只对图像I1、I2均只做了一次全频域的傅里叶变换变换，位移通过离散相位相关方法求得，因此相对于现有技术中的相位相关法而言，无需多次对图像旋转并求傅里叶变换的步骤，大大减少了傅里叶变换的次数，从而节省了计算时间，进而可部署在较低端的计算设备上。

根据本发明的一方面，存在独立的视觉里程计模块和地图匹配模块。视觉里程计模块保证任何时刻都有坐标输出，甚至是在无地图的场景下也能够输出短时间内准确的坐标。而地图匹配模块则在小车运行到已建立地图的区域时能够提供正确的全局坐标。本发明中的视觉里程计模块与地图匹配模块可以在同一线程中进行，亦可以可分线程同时运行，从而可充分利用当前流行的多核计算设备平台。

根据本发明的一方面，通过利用视觉里程计和地图匹配的配准数据，具备一定程度的摄像头俯仰角度自动修正能力，能够有效地减少由于震动、轮子磨损等引发的摄像头相对地面姿态变化而必须由人工校准摄像头的次数，降低后续维护成本。

基于此，已经介绍了本发明提供的基于地面纹理的定位导航方法的实现方法。本发明还提供了一种能够实现上述基于地面纹理的定位导航方法的定位导航装置，请参考图2，图2示出了上述定位导航装置200包括的处理器210、存储器220以及存储在存储器220上并可在处理器上210运行的计算机程序(在图2中未示出)。上述处理器210执行上述计算机程序时能够实现上述所介绍的基于地面纹理的定位导航方法。关于上述定位导航装置200所能够实现的功能，具体请参见上述对于基于地面纹理的定位导航方法能够实现的功能的描述，在此不再赘述。

进一步的，本发明还提供一种AGV小车，请参考图3，本发明所提供的AGV小车300可以包括摄像装置310和上述的定位导航装置200。摄像装置310用以获取地面纹理图像来实现AGV小车300的定位以及导航。定位导航装置200中的处理器210可以是单独设置的独立模块，亦可以是整个AGV小车300的中央处理器。也就是可以通过各种方式实现摄像装置310与定位导航装置200之间的通信，从而能够使得定位导航装置200能够得到可以配准的地面纹理图像，摄像装置310亦可以被控制从而自动校准俯仰参数。

至此，已经描述了本发明提供的一种基于地面纹理的定位导航方法及装置。本发明还提供了一种计算机存储介质，其上存储有计算机程序，当该计算机程序被处理器执行时实现如上述定位导航方法的步骤。

本领域技术人员将可理解，信息、信号和数据可使用各种不同技术和技艺中的任何技术和技艺来表示。例如，以上描述通篇引述的数据、指令、命令、信息、信号、位(比特)、码元、和码片可由电压、电流、电磁波、磁场或磁粒子、光场或光学粒子、或其任何组合来表示。

本领域技术人员将进一步领会，结合本文中所公开的实施例来描述的各种解说性逻辑板块、模块、电路、和算法步骤可实现为电子硬件、计算机软件、或这两者的组合。为清楚地解说硬件与软件的这一可互换性，各种解说性组件、框、模块、电路、和步骤在上面是以其功能性的形式作一般化描述的。此类功能性是被实现为硬件还是软件取决于具体应用和施加于整体系统的设计约束。技术人员对于每种特定应用可用不同的方式来实现所描述的功能性，但这样的实现决策不应被解读成导致脱离了本发明的范围。

结合本文所公开的实施例描述的各种解说性逻辑模块、和电路可用通用处理器、数字信号处理器(DSP)、专用集成电路(ASIC)、现场可编程门阵列(FPGA)或其它可编程逻辑器件、分立的门或晶体管逻辑、分立的硬件组件、或其设计成执行本文所描述功能的任何组合来实现或执行。通用处理器可以是微处理器，但在替换方案中，该处理器可以是任何常规的处理器、控制器、微控制器、或状态机。处理器还可以被实现为计算设备的组合，例如DSP与微处理器的组合、多个微处理器、与DSP核心协作的一个或多个微处理器、或任何其他此类配置。

结合本文中公开的实施例描述的方法或算法的步骤可直接在硬件中、在由处理器执行的软件模块中、或在这两者的组合中体现。软件模块可驻留在RAM存储器、闪存、ROM存储器、EPROM存储器、EEPROM存储器、寄存器、硬盘、可移动盘、CD-ROM、或本领域中所知的任何其他形式的存储介质中。示例性存储介质耦合到处理器以使得该处理器能从/向该存储介质读取和写入信息。在替换方案中，存储介质可以被整合到处理器。处理器和存储介质可驻留在ASIC中。ASIC可驻留在用户终端中。在替换方案中，处理器和存储介质可作为分立组件驻留在用户终端中。

在一个或多个示例性实施例中，所描述的功能可在硬件、软件、固件或其任何组合中实现。如果在软件中实现为计算机程序产品，则各功能可以作为一条或更多条指令或代码存储在计算机可读介质上或藉其进行传送。计算机可读介质包括计算机存储介质和通信介质两者，其包括促成计算机程序从一地向另一地转移的任何介质。存储介质可以是能被计算机访问的任何可用介质。作为示例而非限定，这样的计算机可读介质可包括RAM、ROM、EEPROM、CD-ROM或其它光盘存储、磁盘存储或其它磁存储设备、或能被用来携带或存储指令或数据结构形式的合意程序代码且能被计算机访问的任何其它介质。任何连接也被正当地称为计算机可读介质。例如，如果软件是使用同轴电缆、光纤电缆、双绞线、数字订户线(DSL)、或诸如红外、无线电、以及微波之类的无线技术从web网站、服务器、或其它远程源传送而来，则该同轴电缆、光纤电缆、双绞线、DSL、或诸如红外、无线电、以及微波之类的无线技术就被包括在介质的定义之中。如本文中所使用的盘(disk)和碟(disc)包括压缩碟(CD)、激光碟、光碟、数字多用碟(DVD)、软盘和蓝光碟，其中盘(disk)往往以磁的方式再现数据，而碟(disc)用激光以光学方式再现数据。上述的组合也应被包括在计算机可读介质的范围内。

提供之前的描述是为了使本领域中的任何技术人员均能够实践本文中所描述的各种方面。但是应该理解，本发明的保护范围应当以所附权利要求书为准，而不应被限定于以上所解说实施例的具体结构和组件。本领域技术人员在本发明的精神和范围内，可以对各实施例进行各种变动和修改，这些变动和修改也落在本发明的保护范围之内。

Claims (19)

1.一种基于地面纹理的定位导航方法，其特征在于，包括：

获取当前帧的地面纹理图像；

将所述当前帧的地面纹理图像与参考帧的地面纹理图像进行配准处理的第一配准，所述配准处理系对两张地面纹理图像频谱上的特征点所对应的离散频域至少通过离散相位相关法进行求解以得到两张地面纹理图像之间的相对位移与相对角度；以及

响应于所述第一配准成功，基于所述参考帧的位置信息以及所述参考帧与所述当前帧之间的相对位移与相对角度确定所述当前帧的位置信息。

2.如权利要求1所述的定位导航方法，其特征在于，所述定位导航方法还包括：

响应于所述参考帧与所述当前帧之间的相对位移超出预设阈值，更新所述参考帧的地面纹理图像为所述当前帧的地面纹理图像，更新所述参考帧的位置信息为所述当前帧的位置信息。

3.如权利要求2所述的定位导航方法，其特征在于，所述地面纹理图像为边长为w的正方形；所述超出预设阈值进一步包括：

响应于所述参考帧与所述当前帧之间x方向或y方向的相对位移大于0.5w，则认为所述参考帧与所述当前帧之间的相对位移超出预设阈值。

4.如权利要求1所述的定位导航方法，其特征在于，所述定位导航方法还包括：

响应于所述第一配准失败，基于所述参考帧与上一帧之间的相对位移与相对角度确定所述参考帧与所述当前帧之间的预估相对位移与预估相对角度；

基于所述参考帧的位置信息以及所述参考帧与所述当前帧之间的预估相对位移与预估相对角度确定所述当前帧的位置信息；以及

更新所述参考帧的地面纹理图像为所述当前帧的地面纹理图像，更新所述参考帧的位置信息为所述当前帧的位置信息。

5.如权利要求4所述的定位导航方法，基于所述参考帧与上一帧之间的相对位移与相对角度确定所述参考帧与所述当前帧之间的预估相对位移与预估相对角度进一步包括：

获取所述当前帧的速度；

获取所述上一帧与所述当前帧之间的时间差；以及

基于所述速度、所述时间差以及所述参考帧与上一帧之间的相对位移与相对角度确定所述参考帧与所述当前帧之间的预估相对位移与预估相对角度。

6.如权利要求2或4所述的定位导航方法，其特征在于，响应于所述参考帧的位置信息被更新为所述当前帧的位置信息，所述定位导航方法还包括：

将所述参考帧更新后的地面纹理图像与预设的若干地图帧的地面纹理图像逐一进行所述配准处理的若干第二配准；以及

响应于任意第二配准成功，根据该配准成功的地图帧的位置信息以及该配准成功的地图帧与所述参考帧之间的配准相对位移与配准相对角度校准所述参考帧的位置信息。

7.如权利要求6所述的定位导航方法，其特征在于，响应于任意第二配准成功，所述定位导航方法还包括：

基于所述参考帧校准前的位置信息与该配准成功的地图帧的位置信息获取该配准成功的地图帧与所述参考帧之间的预估相对位移和预估相对角度；

获取所述当前帧的上一帧与更新前的参考帧之间的配准相对位移；

基于该配准成功的地图帧与所述参考帧之间的配准相对角度、预估相对角度以及所述当前帧的上一帧与更新前的参考帧之间的配准相对位移对所述地面纹理图像的摄像装置进行俯仰校正。

8.如权利要求1所述的定位导航方法，其特征在于，所述配准处理进一步包括：

至少通过二维快速傅立叶变换得到两张地面纹理图像频谱上去混叠化的特征点点集P1f、P2f；

基于特征点点集P1f、P2f提取两张地面纹理图像之间的相对角度th，所述相对角度th使得特征点点集P1f与旋转了th度后的特征点点集P2f之间有最多的重合点对PL(i)；以及

获取每个重合点对PL(i)分别在两张地面纹理图像上的相位差ph(i)，以通过离散相位相关法得到相关平面图R，其中

响应于所述相关平面图R中峰值点的峰值量小于预设常数，则配准失败；

响应于所述峰值点的峰值量大于等于所述预设常数，基于所述峰值点在所述相关平面图R中的坐标(x,y)获取所述两张地面纹理图像之间的相对位移。

9.如权利要求8所述的定位导航方法，其特征在于，得到所述特征点点集P1f、P2f进一步包括：

分别对待处理的两张地面纹理图像进行全频域的二维快速傅立叶变换并取模得到两张地面纹理图像的谱功率A1、A2；

提取所述谱功率A1、A2的特征点中特征响应幅值最高的若干点组成特征点点集P1、P2，并记录点集P1、P2中各点的功率；

将所述特征点点集P1、P2旋转c度以得到特征点点集P1c、P2c，使用离散傅立叶变换基于旋转了c度后的两张地面纹理图像获取特征点点集P1c、P2c中各点的功率，其中，c度在3-5度之间；以及

取特征点点集P1和P1c中功率差异最小的若干点组成特征点点集P1f，取特征点点集P2和P2c中功率差异最小的若干点组成特征点点集P2f。

10.如权利要求8所述的定位导航方法，其特征在于，提取所述相对角度th进一步包括：

多次随机选择特征点点集P1f和P2f中的点P1f(i)和P2f(j)，计算P1f(i)和P2f(j)至原点的距离是否接近；

响应于接近，基于点P1f(i)和P2f(j)的坐标计算两张地面纹理图像之间的假定相对角度th’，计算特征点点集P1f与旋转了th’度后的特征点点集P2f之间的重合点对PL(i)的数量；以及

在多个假定相对角度th’中选择使得特征点点集P1f与旋转了th’度后的特征点点集P2f之间的重合点对PL(i)的数量最大的假定相对角度th’为所述相对角度th。

11.如权利要求10所述的定位导航方法，其特征在于，响应于存在两张地面纹理图像的预估相对角度thp，提取所述相对角度th还包括：

在计算得到假定相对角度th’后，判断该假定相对角度th’与所述预估相对角度thp是否接近，响应于接近，计算特征点点集P1f与旋转了th’度后的特征点点集P2f之间的重合点对PL(i)的数量。

12.如权利要求8所述的定位导航方法，其特征在于，响应于存在两张地面纹理图像的预估相对位移(xp,yp)，基于所述峰值点在所述相关平面图R中的坐标获取所述两张地面纹理图像之间的相对位移进一步包括：

分别计算位移(x,y)、(x-w,y)、(x,y-w)、(x-w,y-w)和所述预估相对位移(xp,yp)之间的差异，取位移(x,y)、(x-w,y)、(x,y-w)、(x-w,y-w)中与所述预估相对位移(xp,yp)之间差异最小的位移为所述两张地面纹理图像之间的相对位移；其中

w为所述地面纹理图像的边长。

13.如权利要求8所述的定位导航方法，其特征在于，响应于不存在两张地面纹理图像的预估相对位移，基于所述峰值点在所述相关平面图R中的坐标获取所述两张地面纹理图像之间的相对位移进一步包括：

分别测试两张地面纹理图像的其中之一在旋转了th度、位移了(x,y)、(x-w,y)、(x,y-w)、(x-w,y-w)后，两张地面纹理图像之间的重叠度，取位移(x,y)、(x-w,y)、(x,y-w)、(x-w,y-w)后重叠度最高的位移为所述两张地面纹理图像之间的相对位移；其中

w为所述地面纹理图像的边长。

14.如权利要求1所述的定位导航方法，其特征在于，所述定位导航方法还包括：对地面纹理图像进行预处理，以将原始图像I转化为定义在复数域上的边缘方向均衡化的结果图像Y；其中

所述配准处理的两张地面纹理图像系经过预处理后的两张结果图像。

15.如权利要求14所述的定位导航方法，其特征在于，所述预处理进一步包括：

获取所述原始图像I在x、y方向的梯度图Ix、Iy；

基于所述梯度图Ix、Iy获取所述原始图像I的边缘图E；

基于所述梯度图Ix、Iy和所述边缘图E获取边缘方向图D；

基于所述边缘图E中的N个随机像素获取各个像素值的频数表H，对所述频数表H中的每一项做累加和以得到累加频数表H’；以及

基于所述频数表H、所述累加频数表H’均衡化所述边缘方向图D以得到所述结果图像Y。

16.如权利要求15所述的定位导航方法，其特征在于，所述结果图像Y中像素x,y的像素值Y(x,y)＝(H(E(x,y))/H’(255))^F*D(x,y)；其中

E(x,y)＝Ix(x,y)*Ix(x,y)+Iy(x,y)*Iy(x,y)；

D(x,y)＝(Ix(x,y)+i*Iy(x,y))/E(x,y)，i为复数虚部；以及

F为预设常数。

17.一种基于地面纹理的定位导航装置，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，其特征在于，所述处理器执行所述计算机程序时实现如权利要求1-16中任一项所述基于地面纹理的定位导航方法的步骤。

18.一种AGV小车，其特征在于，所述AGV小车包含如权利要求17所述的基于地面纹理的定位导航装置，以用于所述AGV小车的定位导航。

19.一种计算机可读存储介质，其上存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现如权利要求1-16中任一项所述基于地面纹理的定位导航方法的步骤。

Images