CN109141442B

CN109141442B - 基于uwb定位与图像特征匹配的导航方法和移动终端

Info

Publication number: CN109141442B
Application number: CN201811040673.4A
Authority: CN
Inventors: 高子庆
Original assignee: Individual
Current assignee: Shenzhen Reeman Intelligent Equipment Co ltd
Priority date: 2018-09-07
Filing date: 2018-09-07
Publication date: 2022-05-17
Anticipated expiration: 2038-09-07
Also published as: CN109141442A

Abstract

本发明提供一种基于UWB定位与图像特征匹配的导航方法和移动终端，包括：获取当前位置下的定位信息以及匹配的导航路线，并同时根据定位信息获取与定位信息映射的预设地图所映射的导航路线；获取移动终端当前的第一位姿信息，获取移动终端当前位置状态下的UWB定位信息；判断第一位姿信息与UWB定位信息之间的差值是否超出预设阈值；当超出预设阈值，重新进行定位，并重新获取导航路线，以使移动终端按照新的导航路线移动后，推算出的第一位姿信息与移动终端本身的实际位置的距离在预设阈值内。本发明将定位结果与实际位置进行比对，便于及时发现误差，及时调整；搭建预设地图，在实际移动中与预设地图对比，达到三维空间避障的目的。

Description

基于UWB定位与图像特征匹配的导航方法和移动终端

技术领域

本发明涉及车辆安全领域，具体而言，本发明涉及一种基于UWB定位与图像特征匹配的导航方法和装置。

背景技术

移动机器人在导航过程中，定位和导航都很重要。如果定位偏差大，会导致机器人导航出现乱走甚至导航失败的情况，所以导航对定位的依赖非常大，而且对定位精度的要求也比较高。同时，导航算法的质量也直接关系到机器人行走的步态。

目前室外的定位技术主要是卫星定位(如：gps)，和手机基站定位。但是在室内卫星信号太弱，而且精度不够，基本不能定位，手机基站定位误差非常大。所以需要在信号不好的地方和室内的时候，就需要采用其他的定位方式。

目前的室内定位技术主要有：码盘，惯性传感器，Wi-Fi定位，蓝牙定位，uwb无线定位，匹配定位。他们各有优缺点，一般都需要结合多种方法才能完成精准的定位。

1.码盘、惯性传感器：这种定位方法精度较高，但有累积误差，随着行驶时间、距离的增加，误差也不断增大。此外，这两者都是计算相对位姿，无法直接获取绝对位姿。因此这两者适合在知道精确的绝对位姿后持续获取精确的位姿，当进行长时间、长距离的精确定位时，需要经常校正。

2.Wi-Fi定位、蓝牙定位、uwb无线定位：能够获取绝对位置，但是没有姿态信息，精度较差且不稳定，都会受障碍物和电磁环境的影响，比如穿墙、穿过金属物体后或周围电磁环境复杂时精度变差。但是其中精度最高，抗干扰最强的定位方式就是uwb无线定位。Uwb理论定位误差可以在10cm以内，在实际的使用虽然存在干扰，但误差一般都在0.5m以内，满足粗略定位的需求。

3.匹配定位：包括激光匹配，图像匹配。预先保存地图，利用地图中的特征点来进行匹配定位，能够获取精确的位姿，但运算量较大，很难做到实时获取位姿，并且在遇到相似的场景时会得到多个匹配结果。

以上方案各有优劣，但目前很少有能够取长补短、同时兼顾性价比的导航定位方案，这一领域的市场需求并未得到满足。

发明内容

本发明的目的旨在至少能解决上述的技术缺陷之一，公开一种能够精准定位的导航方法和装置。

一方面本发明公开一种基于UWB定位与图像特征匹配的导航方法，包括：

获取当前位置下的定位信息，并同时根据定位信息获取与定位信息映射的预设地图所映射的导航路线；

获取移动终端当前的第一位姿信息，所述第一位姿信息为前一测量状态下获得的第一位姿信息与当前位置状态下的第二位姿信息结合所得，所述第二位姿信息为所述移动终端当前移动状态下通过视觉里程计和/或码盘里程计结合得到；初始位置的第一位姿信息为定位信息；

获取移动终端当前位置状态下的UWB定位信息；

判断所述第一位姿信息与所述UWB定位信息之间的差值是否超出预设阈值；

当超出预设阈值，重新进行定位，并重新获取导航路线，以使所述移动终端按照新的导航路线移动后，推算出的第一位姿信息与移动终端本身的实际位置的距离在预设阈值内。

进一步的，所述定位信息的获取方法包括：

基于UWB定位方式获取当前状态下的UWB定位信息；

在所述UWB定位信息的范围值内，获取当前移动终端拍摄的3D图像信息的第一特征点及其空间位置，以及在预设地图中所述UWB定位信息的范围值内的第二特征点及其空间位置；

比较所述第一特征点及其空间位置与所述第二特征点及其空间位置，得到所述定位信息。

进一步的，所述预设地图的搭建方法包括：

获取移动终端拍摄的3D图像信息；

针对所述3D图像信息提取表征物体轮廓的特征点，计算得到所述特征点的描述子及空间位置；

将所有所述特征点转换成点云合并成三维点云图，区分障碍物、可通行区域和未知区域搭建成预设地图。

进一步的，所述获取视觉里程计的方法包括：

获取相邻两个时间点的两帧画面的特征点及其空间位置；

采用最小化误差法获取所述两帧画面的特征点及其空间位置的最优旋转矩阵和位移矢量；

通过所述最优的旋转矩阵和位置矢量获取相邻两帧画面的第一相对位姿变化值；

优化处理所述第一相对位姿变化值得到视觉里程计。

进一步的，所述点云的生成方法包括：

所述点云的生成方法包括：

根据第二位姿信息以及获得的最优的旋转矩阵和位置矢量，根据变换矩阵拼接而成，所述变换矩阵为：

R_3×3表示一个3×3的旋转矩阵，其为一个正交阵；T_3×1为3×1的位移矢量。O_1×3为1×3的缩放矢量。

进一步的，所述提取表征物体轮廓的特征点的方法包括：采用FAST算法获取所述3D图像信息中的特征点。

进一步的，计算所述特征点的描述子的方法包括：采用图像金字塔的方式将3D图像缩放，并分别记录同一特征点在不同缩放比例下的描述子。

进一步的，所述导航路线的获取方法包括：

通过目标位置和所述障碍物位置规划成全局路径，

根据当前获取的3D图像的特征点和空间位置计算前方障碍物位置，规划局部路径。

进一步的，所述导航路线为到达所述目标位置的在所述可通行区域内的最短路径。

另一方面，本发明公开一种移动终端，包括3D摄像装置、码盘和UWB定位装置和处理器，所述处理器配置上述任意一项所述的基于UWB定位与图像特征匹配的定位导航方法，所述3D摄像装置码盘和UWB定位装置分别与处理器电连接通信。

另一方面，本发明公开一种非临时性计算机可读存储介质，当所述存储介质中的指令由移动终端的处理器执行时，使得移动终端能够执行一种基于社交软件的信息显示的控制方法，所述方法包括上述任意一项所述的基于UWB定位与图像特征匹配的导航方法。

本发明的有益效果为：

1)通过移动终端在移动过程中进行实时定位和导航，将定位结果与实际位置进行比对，以便于及时发现误差，并及时调整，达到减小误差的目的；

2)搭建预设地图，并在实际移动中与预设地图做比较，达到三维空间避障的目的；

3)通过uwb定位方式快速锁定目标区域，再在小范围内用视觉匹配精确定位，同时满足了定位的实时性与准确性，对传感器性能无特殊要求，有效改善了现有导航定位方案的精度、速度、稳定性与成本无法兼顾的现状。

本发明附加的方面和优点将在下面的描述中部分给出，这些将从下面的描述中变得明显，或通过本发明的实践了解到。

附图说明

本发明上述的和/或附加的方面和优点从下面结合附图对实施例的描述中将变得明显和容易理解，其中：

图1为一个实施例的定位导航方法流程图；

图2为一个实施例的预设地图的搭建方法示意图；

图3为一个实施例中定位信息的获取方法的流程图；

图4为一个实施例中视觉里程计的获取方法；

图5为本实施例定位阶段原理示意图；

图6为本发明一实施例中的移动终端结构示意图。

具体实施方式

下面详细描述本发明的实施例，所述实施例的示例在附图中示出，其中自始至终相同或类似的标号表示相同或类似的元件或具有相同或类似功能的元件。下面通过参考附图描述的实施例是示例性的，仅用于解释本发明，而不能解释为对本发明的限制。

本技术领域技术人员可以理解，除非特意声明，这里使用的单数形式“一”、“一个”、“所述”和“该”也可包括复数形式。应该进一步理解的是，本发明的说明书中使用的措辞“包括”是指存在所述特征、整数、步骤、操作、元件和/或组件，但是并不排除存在或添加一个或多个其他特征、整数、步骤、操作、元件、组件和/或它们的组。应该理解，当我们称元件被“连接”或“耦接”到另一元件时，它可以直接连接或耦接到其他元件，或者也可以存在中间元件。此外，这里使用的“连接”或“耦接”可以包括无线连接或无线耦接。这里使用的措辞“和/或”包括一个或更多个相关联的列出项的全部或任一单元和全部组合。

本技术领域技术人员可以理解，除非另外定义，这里使用的所有术语(包括技术术语和科学术语)，具有与本发明所属领域中的普通技术人员的一般理解相同的意义。还应该理解的是，诸如通用字典中定义的那些术语，应该被理解为具有与现有技术的上下文中的意义一致的意义，并且除非像这里一样被特定定义，否则不会用理想化或过于正式的含义来解释。

本技术领域技术人员可以理解，这里所使用的“终端”、“终端设备”既包括无线信号接收器的设备，其仅具备无发射能力的无线信号接收器的设备，又包括接收和发射硬件的设备，其具有能够在双向通信链路上，执行双向通信的接收和发射硬件的设备。这种设备可以包括：蜂窝或其他通信设备，其具有单线路显示器或多线路显示器或没有多线路显示器的蜂窝或其他通信设备；PCS(Personal Communications Service，个人通信系统)，其可以组合语音、数据处理、传真和/或数据通信能力；PDA(Personal Digital Assistant，个人数字助理)，其可以包括射频接收器、寻呼机、互联网/内联网访问、网络浏览器、记事本、日历和/或GPS(Global Positioning System，全球定位系统)接收器；常规膝上型和/或掌上型计算机或其他设备，其具有和/或包括射频接收器的常规膝上型和/或掌上型计算机或其他设备。这里所使用的“终端”、“终端设备”可以是便携式、可运输、安装在交通工具(航空、海运和/或陆地)中的，或者适合于和/或配置为在本地运行，和/或以分布形式，运行在地球和/或空间的任何其他位置运行。

本技术领域技术人员可以理解，这里所使用的远端网络设备，其包括但不限于计算机、网络主机、单个网络服务器、多个网络服务器集或多个服务器构成的云。在此，云由基于云计算(Cloud Computing)的大量计算机或网络服务器构成，其中，云计算是分布式计算的一种，由一群松散耦合的计算机集组成的一个超级虚拟计算机。本发明的实施例中，远端网络设备、终端设备与WNS服务器之间可通过任何通信方式实现通信，包括但不限于，基于3GPP、LTE、WIMAX的移动通信、基于TCP/IP、UDP协议的计算机网络通信以及基于蓝牙、红外传输标准的近距无线传输方式。

本发明公开一种基于UWB定位与图像特征匹配的导航方法，UWB(Ultra Wideband)是一种无载波通信技术，利用纳秒至微秒级的非正弦波窄脉冲传输数据，常被用于室内短距离定位。

请参阅图1，本发明的定位导航方法的步骤包括：

S100、获取当前位置下的定位信息，并同时根据定位信息获取与定位信息映射的预设地图所映射的导航路线；

导航路线为到达目标位置在预设地图内的通行区域内的移动的路径，移动终端沿着导航路线进行移动，以达到目标位置。需要说明的是导航路线的获得基于存储好的预设地图，输入目标位置后，系统自动得出来的，其可以是任何一条可以从移动终端当前位置移动到目标位置的路径，但是优选的方案是，采用最短的能够避开障碍物的路径作为导航路线。

在本发明的一实施例中，导航路线获取方式为：通过目标位置和预设地图上的障碍物位置规划成全局路径，再根据移动终端当前获取的3D图像的特征点和空间位置计算前方障碍物位置，规划局部路径。

预设地图，为预先保存在系统中的包含起始位置和目标位置的路径分布集合，其包含障碍物、可通行区域和未知区域。

在本申请中，预设地图的获得，是通过移动终端初次在目标区域进行移动后，根据自身的UWB定位以及图像特征匹配定位而搭建起来的。请参阅图2，所述预设地图的搭建方法包括：

S110、获取移动终端拍摄的3D图像信息；

本发明公开的移动终端为可以移动，同时带有摄像装置以及UWB定位装置的设备，优选的一种方案为可自动移动的机器人，该移动设备设置3D摄像装置，以便于在移动过程中进行拍摄。进一步的，该3D摄像装置包括双目摄像头，通过双目摄像头实现3D图像的拍摄。

S120、针对所述3D图像信息提取表征物体轮廓的特征点，计算得到所述特征点的描述子及空间位置；

特征点提取是图像识别过程中获取图像的关键步骤。在一实施例中，采用FAST算法来检测特征点。具体方法包括：对于3D图像中的每一个像素点，选取与它距离2个像素的16个像素点，如果其中有连续10个以上的像素点与中心像素点灰度值的差值超过一个阈值，则认为中心像素点是一个特征点，在此过程中，为了加速运算，我们首先判断16个像素点中等距的4个点，如第4、8、12、16个点，如果有2个以上像素点与中心像素点灰度值差别大，则中心像素点有可能是特征点，继续运算；否则该点不可能是特征点，重新选择下一个中心像素点；以此方式得到表征图像轮廓信息的所有特征点。

当获取了特征点，则可计算所有特征点的BRIEF特征描述子，所述特征点的描述子的获取方法包括：

以特征点P为中心，取一定大小的Patch邻域：获取像素点的灰度值，以特征点P为圆心，指定长度为半径做圆；

在这个领域内随机取N个点对，然后对这2*N点分别做高斯平滑。定义T测试，比对N对像素点的灰度值的大小；

比如，在圆内取N个点对，以5像素为半径，N＝4为例，每个点对表示成：P1(A,B),P2(A,B),P3(A,B),P4(A,B),每个像素点的灰度表示成：I_A、I_B，

定义运算T：

对每个点对进行T操作，得到的结果即为该特征点的BRIEF描述子，如：

T(P1(A,B))＝0,

T(P2(A,B))＝0,

T(P3(A,B))＝1,

T(P4(A,B))＝0,

将上述步骤得到的N个二进制码串组成一个N维向量，即该特征点的描述子为0010。

由于单纯计算出来的BRIEF描述子无法用来匹配旋转过的特征点，不具备旋转不变性，所以我们根据选区的质心重新定义坐标轴，使其不依赖于原图的方向。首先，令选区内每个像素的质量等于其灰度值，然后根据下面的公式计算其质心的坐标：

其中Q为质心，R为选区半径，I(x,y)为该点的质量。计算出质心后，令特征点为坐标原点，原点到质心的方向为X轴正方向，此时即使原图旋转，坐标轴也会跟着旋转，解决了旋转一致性的问题。

进一步的，还包括：采用图像金字塔的方式将3D图像缩放，并分别记录同一特征点在不同缩放比例下的描述子。

上面计算出的描述子在面对不同距离、尺寸的物体时性能较差，存在尺度一致性的问题，因此，我们采用图像金字塔的方法，将一幅图像缩放至1:4、1:16、1:64等不同尺寸，分别记录同一特征点在不同缩放比例下的描述子。

由于我们使用的是双目深度摄像头，因此在提取出特征点的同时，也可以获得该特征点的空间位置。空间坐标[x,y,z]与图像坐标[u,v,d](d为深度)的对应关系如下：

z＝d/s

x＝(u-c_x)·z/f_x

y＝(v-c_y)·z/f_y

其中，f为相机在x、y两个轴上的焦距，c为相机光圈中心，s为深度图的缩放比例，通常为1000，其中，标准单位是米，深度单位是毫米。

S130、将所有所述特征点转换成点云合并成三维点云图，区分障碍物、可通行区域和未知区域搭建成预设地图。

通过获取各个时间段的3D图像生成的特征点及其空间位置，可将所有特征点转换成点云，将这些点云合并起来得到了三维重建的点云图。然后，根据预设的摄像头高度确定地平面，将所有地平面附近的特征点标记为地面，即可通行区域；其余的特征点中，所有高度不超过机器人身高的特征点标记为障碍物；其他没有特征点的区域标记为未知区域，即可得到一幅由障碍物、可通行区域、未知区域构成的二维地图，该地图也就是预设地图。

定位信息为获取的移动终端当前的位置信息，请参阅图3，定位信息的获取方法包括：

S140、基于UWB定位方式获取UWB定位信息；

移动终端中的UWB定位通过UWB装置与部署在周围的UWB基站共同获得，在本实施例中，在移动空间范围内，部署四个UWB基站，在移动终端上安装UWB标签。移动终端移动过程中，UWB标签发送广播，UWB基站接收到信号后作出回复，相关的处理装置将发消息的时间间隔的1/2和电磁波的传输速度相乘可以得到UWB标签与UWB基站的距离，以各个基站的坐标和标签与之的距离为球心和半径作球，得出的其中三个球会有两个交点P1，P2,请参阅图5，分别以第一UWB基站J1、第二UWB基站J2和第三UWB基站J3作圆。由于各硬件的性能和环境干扰等因素的不稳定性，这两个交点不能保证落在第四个UWB基站作出的球面上，假设UWB基站标签测量到的第四个UWB基站的距离为S，分别计算P1，P2到第四个基站的距离S1和S2，选取与S更接近的结果作为本次定位的结果，由此可确定移动终端的大致位置P。若测量出的第四个UWB基站J4的距离S＝6.8m，则S1与S相差1.8m,S2与S相差0.2m，则P2点为本次定位的结果位置。

S150、在所述UWB定位信息的范围值内，获取当前移动终端拍摄的3D图像信息的第一特征点及其空间位置，以及在预设地图中该UWB定位信息的范围值内的第二特征点及其空间位置；

由于UWB定位方式速度快，却精确度较差，无法获取方向，所以只能用这种方式来确定一个大致的范围，或者用于大致判断其在导航过程中有没有偏离太远。若需要精确获取第一位姿信息，还需要通过图像特征匹配的方式结合起来使用。其中图像特征匹配是关于获取的UWB定位范围内当前移动终端拍摄的3D图像信息的第一特征点及其空间位置和在该UWB定位信息的范围值内预设地图中所表征的第二特征点及其空间位置。

S160、比较所述第一特征点及其空间位置与所述第二特征点及其空间位置，得到所述第一位姿信息。

根据上述步骤S120的方法，可以获得第一特征点及其空间位置，同样的方法，可得到第二特征点及其空间位置。第二特征点及其空间位置是在搭建预设地图过程中通过3D摄像装置拍摄的3D图像所提取的特征点，相对而言，其可以作为一个参考值，将当前移动状态下获得的第一特征点及其空间位置与第二特征点及其空间位置对比，则可获得一个绝对位姿，该位姿信息则为第一位姿信息。

S200、获取移动终端当前的第一位姿信息，所述第一位姿信息为前一测量状态下获得的第一位姿信息与当前位置状态下的第二位姿信息结合所得，所述第二位姿信息为所述移动终端当前移动状态下通过视觉里程计和/或码盘里程计结合得到；初始位置的第一位姿信息为定位信息；

第一位姿信息为移动终端在移动过程中当前位置的精确位置，初始状态下的第一位姿信息则为定位信息，随着移动终端的移动，第一位姿信息会发生变化。本申请中，第一位姿信息为前一测量状态下获得的第一位姿信息与当前位置状态下的第二位姿信息结合所得。在一实施例中，第二位姿信息为移动终端移动过程中在测量单位时间内的相对位姿变化，故将前一测量状态下的第一位姿信息与当前状态下获得的移动终端发生的相对位姿变化相加则可获得当前状态的第一位姿信息。

所述第二位姿信息为所述移动终端当前移动状态下通过视觉里程计和/或码盘里程计结合得到；之所以需要二者结合在于，码盘精度高，有方向，但只能获取一段时间内的相对位姿变化，并且遇到轮子打滑的情况就不可靠了。因此，要跟视觉里程计配合使用。

在一实施例中，第二位姿信息通过视觉里程计和/或码盘里程计结合的规则包括：当视觉里程计与码盘里程计两者相差不大时，将视觉里程计与码盘里程计的平均值作为第二位姿信息；当视觉里程计与码盘里程计两者相差较大时，取视觉里程计的值作为第二位姿信息。

视觉里程计精度较高，但易受环境光线和物体移动情况影响，可以获取一段时间内的相对位姿。另外，视觉里程计是将当前帧与上一帧进行特征匹配、计算相对位姿。具体的，请参阅图4，获取视觉里程计的方法包括：

S210、获取相邻两个时间点的两帧画面的特征点及其空间位置；

相邻两个时间点的两帧画面通常是指前一测量状态下获得的帧画面以及当前状态下获得的帧画面，通过上述S120的步骤，可获得该相邻两个帧画面的特征点及其空间位置。但是，获取视觉里程计的过程中，需要对前后帧画面进行匹配，判断是否为相似帧画面，判断的方法也是采用特征比对的方式，依据上述的方式提取出特点，设定一个阈值，然后比较两个特征点的相似度，当相似度大于阈值时，认为匹配成功。

比如，假设阈值为85％，特征点A、B的描述子为：A：01010110，B：01010111，只有一位不同，相似度为87.5％，大于85％。则A和B匹配成功，认定为相似帧画面。

S220、采用最小化误差法获取所述两帧画面的特征点及其空间位置的最优旋转矩阵和位移矢量；

对于两个相邻帧F1,F2，假设已经获得了两组匹配的特征点P,Q：

P＝{P1,P2,…,Pn}∈F1

Q＝{Q1,Q2,…,Qn}∈F2

通过一个旋转矩阵R和位移矢量t，将两帧图像对应起来：

而由于误差的存在，上式等号不可能成立，所以采用最小化误差的方法来求解：

根据上式使用PnP方法求得最优的旋转矩阵r和位置矢量t，即可算出每帧图像的特征点的空间位置。

S230、通过所述最优的旋转矩阵和位置矢量获取相邻两帧画面的第一相对位姿变化值；

根据上述步骤S250的方法，获得前后帧画面的特征点的精确的空间位置后，二者空间位置关系的差值，则为该相邻两帧画面的第一相对位姿变化值。

S240、优化处理所述第一相对位姿变化值得到视觉里程计。

根据步骤S220中使用PnP方法求得最优的旋转矩阵R和位移矢量t，即可算出每帧图像的特征点的空间位置和两帧图像的相对位姿变化。假设环境是固定的，那么将相邻图像的位姿变化取反，即可求得机器人的位姿变化，即视觉里程计。

而由于这样计算的里程计存在累积误差，所以接下来采用以下方式进行优化。公式中的节点V代表机器人的各个姿态，边E代表姿态之间的变换：

该矩阵的左上部分是一个3×3的旋转矩阵，它是一个正交阵。右上部分是3×1的位移矢量。左下是1×3的缩放矢量，在SLAM中通常取成0，因为现实中物体不太可能迅速改变体积。

由于误差的存在，Tk,k+1+Tk+1,k+2≠Tk,k+2，因此我们用下面的公式最小化误差：

其中

表示x_i的估计值，以动态的x^*的幅度调整x以降低E，若上式收敛，则x^*会越来越小，E会逐渐收敛，当降低至一个可以接受的阈值时，即可认为此时的位姿时可靠的。然后将视觉里程计与轮子码盘的里程计数据结合，可以得到一个相对准确的位姿。

进一步的，基于上述步骤S220的方法，可获得最优的旋转矩阵R和位移矢量t，基于上述的最优的旋转矩阵R和位移矢量t以及第二位姿信息，根据变换矩阵则可拼接出点云。

所变换矩阵为：

R_3×3表示一个3×3的旋转矩阵，其为一个正交阵；T_3×1为3×1的位移矢量。O_1×3为1×3的缩放矢量，在SLAM中通常取成0。

将所有特征点转换成点云合并得到三维重建的点云图。然后，根据预设的摄像头高度确定地平面，将所有地平面附近的特征点标记为地面，即可通行区域；其余的特征点中，所有高度不超过机器人身高的特征点标记为障碍物；其他没有特征点的区域标记为未知区域，即可得到一幅由障碍物、可通行区域、未知区域构成的2维地图。

S300、获取移动终端当前位置状态下的UWB定位信息；

需要说明的是，本申请是基于UWB定位信息与图形特征匹配进行导航的，故在移动终端中应当设置有UWB定位装置，该UWB定位装置可以安装在移动终端的任意位置，通常采用UWB天线来进行定位信号的接收与发送。UWB定位装置获取的UWB定位信息用以结合视觉特征获取精确位的第一姿信息。

S400、判断所述第一位姿信息与所述UWB定位信息之间的差值是否超出预设阈值；

第一位姿信息为前一测量状态下获得的第一位姿信息与当前位置状态下的第二位姿信息结合所得，第二位姿信息通过所述移动终端当前移动状态下通过视觉里程计和/或码盘里程计结合得到，当获取了第一位姿信息后，移动终端会通过内置的UWB定位装置进行当前位置的定位，以获得当前位置的UWB定位信息，UWB定位信息获取的只是一个范围值，而第一位姿信息为计算得到的较为准确的值，但是由于第一位姿信息在经过对第二位姿信息进行不断的累积的过程中，可能会出现偏差，故需要设置一个预设阈值，在每次测量状态下都要将计算得到的第一位姿信息与UWB定位信息进行一个差值对比，判断该差值是否在预设阈值内，以判断移动终端是否脱离导航路线。

S500、当超出预设阈值，重新进行定位，并重新获取导航路线，以使所述移动终端按照新的导航路线移动后，推算出的第一位姿信息与移动终端本身的实际位置的距离在预设阈值内。

基于上述步骤S400的原理，本申请中，当第一位姿信息与所述UWB定位信息之间的差值超出预设阈值时，则判定当前导航路线出现异常，可能导致移动终端脱离预设的轨迹，需要重新定位，并重新获取导航路线，以调整导航路线，直到计算出的第一位姿信息与当前的UWB定位信息的差值在预设阈值范围内，以此确保移动终端的快速精准定位和移动。

另一方面，本发明还公开一种移动终端100，请参阅图6，包括3D摄像装置200、码盘300和UWB定位装置400和处理器(图未示)，所述处理器配置上述任意一项所述的基于UWB定位与图像特征匹配的定位导航方法，所述3D摄像装置码盘和UWB定位装置分别与处理器电连接通信，所述处理器还可以进一步与其他终端或者网络设备有线或无线连接，以进行数据通信，还可通过其他终端或者网络设备进行控制和数据查询。

优选的一种方案为，移动终端100为一种可移动的机器人，所述3D摄像装置200可采用双目摄像头，通过双目摄像头实现3D图像的拍摄,UWB定位装置400安装在移动终端100上，码盘300设置在移动终端100的下端，与移动终端的滑轮500连接。

一种非临时性计算机可读存储介质，当所述存储介质中的指令由移动终端的处理器执行时，使得移动终端能够执行一种基于社交软件的信息显示的控制方法，所述方法包括上述任意一项所述的基于UWB定位与图像特征匹配的导航方法。

应该理解的是，虽然图1的流程图中的各个步骤按照箭头的指示依次显示，但是这些步骤并不是必然按照箭头指示的顺序依次执行。除非本文中有明确的说明，这些步骤的执行并没有严格的顺序限制，其可以以其他的顺序执行。而且，图1中的至少一部分步骤可以包括多个子步骤或者多个阶段，这些子步骤或者阶段并不必然是在同一时刻执行完成，而是可以在不同的时刻执行，其执行顺序也不必然是依次进行，而是可以与其他步骤或者其他步骤的子步骤或者阶段的至少一部分轮流或者交替地执行。

以上所述仅是本发明的部分实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。

Claims

1.一种基于UWB定位与图像特征匹配的导航方法，其特征在于，包括：

获取移动终端当前位置状态下的UWB定位信息；

当超出预设阈值，重新进行定位，并重新获取导航路线，以使所述移动终端按照新的导航路线移动后，推算出的第一位姿信息与移动终端本身的实际位置的距离在预设阈值内；

所述定位信息的获取方法包括：

基于UWB定位方式获取当前状态下的UWB定位信息；

2.根据权利要求1所述的基于UWB定位与图像特征匹配的导航方法，其特征在于，所述预设地图的搭建方法包括：

获取移动终端拍摄的3D图像信息；

3.根据权利要求1所述的基于UWB定位与图像特征匹配的导航方法，其特征在于，获取视觉里程计的方法包括：

获取相邻两个时间点的两帧画面的特征点及其空间位置；

优化处理所述第一相对位姿变化值得到视觉里程计。

4.根据权利要求2所述的基于UWB定位与图像特征匹配的导航方法，其特征在于，所述点云的生成方法包括：

R_3×3表示一个3×3的旋转矩阵，其为一个正交阵；T_3×1为3×1的位移矢量，O_1×3为1×3的缩放矢量。

5.根据权利要求2所述的基于UWB定位与图像特征匹配的导航方法，其特征在于，所述提取表征物体轮廓的特征点的方法包括：采用FAST算法获取所述3D图像信息中的特征点。

6.根据权利要求3所述的基于UWB定位与图像特征匹配的导航方法，其特征在于，计算所述特征点的描述子的方法包括：采用图像金字塔的方式将3D图像缩放，并分别记录同一特征点在不同缩放比例下的描述子。

7.根据权利要求2所述的基于UWB定位与图像特征匹配的导航方法，其特征在于，所述导航路线的获取方法包括：

通过目标位置和所述障碍物位置规划成全局路径，

8.根据权利要求7所述的基于UWB定位与图像特征匹配的导航方法，其特征在于，所述导航路线为到达所述目标位置的在所述可通行区域内的最短路径。

9.一种移动终端，其特征在于，包括3D摄像装置、码盘和UWB定位装置和处理器，所述处理器配置上述权利要求1-8任意一项所述的基于UWB定位与图像特征匹配的导航方法，所述3D摄像装置码盘和UWB定位装置分别与处理器电连接通信。